1/1 


AD-A174  171  COMPUTER  SIMULATION  AND  EXPERIMENTAL  VALIDATION  OF  A 
DVNAMIC  MODEL  CEOUIV  <U>  NAVAL  POSTGRADUATE  SCHOOL 
MONTEREV  CA  R  P  PETROXA  JUN  SO 


UNCLASSIFIED 


F/G  9/2 


NL 


V 


■^pROCOPY  RESOLUTION  TEST  CHART 

HHTfONAl  BUREAU  OF  STANDARDS-I963.A 

i 


3HC  FILE  COHf  AD-A174  171 


NAVAL  POSTGRADUATE  SCHOOL 

Monterey,  California 


DTIC 


COMPUTER  SIMULATION  AND  EXPERIMENTAL 
VALIDATION  OF  A  DYNAMIC  MODEL 
(EQUIVALENT  RIGID  LINK  SYSTEM)  ON 
A  SINGLE-LINK  FLEXIBLE  MANIPULATOR 

by 

Robert  P.  Petroka 
June  1986 

Thesis  Advisor:  Liang-Wey  Chang 

Approved  for  public  release;  distribution  is  unlimited. 

86  11  19  007 


THESIS 


\y,V,V,y,y»*iSy,y 


REPORT  DOCUMENTATION  PAGE 

la  REPORT  SECURITY  CLASSIFICATION 

UNCLASSIFIED 

’b  RESTRICTIVE  MARKINGS 

3a  SECURITY  Classification  AUTHORITY 

3  DISTRIBUTION /AVAILABILITY  OF  REPORT 

Approved  for  public  release; 
distribution  is  unlimited. 

tb  DECLASSIFICATION /DOWNGRADING  SCHEDULE 

' 

4  PERFORMING  ORGANIZATION  REPORT  NUM8ER<S) 

_ _ 

5  MONITORING  ORGANIZATION  REPORT  NUM8ER(S) 

6a  NAME  OF  PERFORMING  ORGANIZATION 

Naval  Postgraduate  School 

6b  OFFICE  SYMBOL 
(If  applicable) 

69 

7a  NAME  OF  MONITORING  ORGANIZATION 

Naval  Postgraduate  School 

6<  ADDRESS  (City.  Stite.  ind  ZIP  Code) 

Monterey,  California  93943-5000 

7b  ADDRESS  (City,  State,  ind  ZIP  Code) 

Monterey,  California  93943-5000 

3a  NAME  OF  FUNDING /SPONSORING 
ORGANIZATION 

8b,  OFFICE  SYMBOL 
(If  ipplidble) 

9  PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 

3c  ADDRESS  (Cry,  State.  and  ZIPCode) 


10  SOURCE  OF  FUNDING  NUMBERS 


PROGRAM 
ELEMENT  NO 


PROJECT 

task 

NO 

NO 

WORK  UNIT 
ACCESSION  NO 


1 '  title  (include  Security  Classification) 

COMPUTER  SIMULATION  AND  EXPERIMENTAL  VALIDATION  OF  A  DYNAMIC  MODEL 
(EQUIVALENT  RIGID  LINK  SYSTEM)  ON  A  SINGLE-LINK  FLEXIBLE  MANIPULATOR 


■;  personal  author(S) 

Petroka,  Robert  P. 


'  3  J  TTPS  OF  REPORT 

Master's  Thesis 


13b  Time  COVERED 
FROM  TO 


14  OATE  OF  REPORT  (Year,  Month,  Day)  15  PAGE  COUNT 

1986  June  92 


_ _ COSATl  CODES  I  18  SUBJECT  TERMS  (Continue  on  reverse  if  necessity  »nd  identify  by  block  number ) 

3  |  group  I  subgroup  |  Flexible  Manipulator,  Manipulator,  Robotics, 

Dynamic  Model,  Robot 


^  -93' RAC "  ! Continue  on  reverse  if  necessity  and  identify  by  block  number) 

-^■Flexibility  effects  on  robot  manipulator  design  and  control  are 

typically  ignored  which  is  justified  when  large,  bulky  robotic  mechanisms 

are  moved  at  slow  speeds.  However,  when  increased  speed  and  improved 

!  accuracy  is  desired  in  robot  system  performance  it  is  necessary  to 

J 

i  consider  flexible  manipulators.  This  project  simulates  the  motion  of 
j  a  single-link,  flexible  manipulator  using  the  Equivalent  Rigid  Link 


. :  j  i':‘  y.~  on  availabil  ty  of  abstract 

Z  ‘.C-ASS  F'CD'IjNL  ViTED  □  Same  AS  RPT  □  DTiC  USERS 

21  abstract  security  classification 

UNCLASSIFIED 

'.avf  PESPOvSiaiE  ‘.Div.oual 

Liang-Wey  Chang  , 

l22b  TELEPHONE  (Include  Area  Code) 

408-649-1036 

CD 

> 

L/' 

SO 

'JCT\ 

t-i 

00  FORM  1473.  84  mar 


33  APR  edition  may  be  oied  jum  e«nausted 
Ail  other  edit  om  are  obsolete 
1 


SECURITY  CLASSiF  CAT'QN  OF  S  RACE 


i 


lift: 

Ifc 


P 


Rj; 

B 


i 

J!v 


% 


SCCUftITV  CL  ASSlFIC  ATtON  Of  THIS  *>*!)(  (VlM  D«(  faf«<4 

System  dynamic  model  and  experimentally  validates  the 
computer  simulation  results.  Validation  of  the  flexible 
manipulator  dynamic  model  is  necessary  to  ensure  confidence 
of  the  model  for  use  in  future  design  and  control  applications 
of  flexible  manipulators. 

r* 

\ 


SCCUMlTv  ClAtSlFlC  ATlOW  O  f  THIS  PAGErW***  Dm  f  Kntmtmd) 


Approved  for  public  release;  distribution  is  unlimited 


COMPUTER  SIMULATION  AND  EXPERIMENTAL  VALIDATION 
OF  A  DYNAMIC  MODEL  (EQUIVALENT  RIGID  LINK  SYSTEM)- 
ON  A  SINGLE-LINK  FLEXIBLE  MANIPULATOR 


by 


Robert  P.  Petroka 

Lieutenant  Commander,  United  States  Navy 
B.S.,  United  States  Naval  Academy,  1976 


Submitted  in  partial  fulfillment  of  the 
requirements  for  the  degrees  of 

MASTER  OF  SCIENCE  IN  MECHANICAL  ENGINEERING 

and 

MECHANICAL  ENGINEER 


from  the 

NAVAL  POSTGRADUATE  SCHOOL 
June  19  86 


Author: 


Approved  by: 


Robert  P.  Petroka 


L.W.  Chang,  Thesis  Advisor 


D.L.  Smith,  Second  Reader 

U  uio  tu 


Paul  Z.  Mar to,  Ch ai rman , 
Department 'of  Mechanical  Engineering 


/ 


01  0-/-A 


Jdhn  N.  Dyer, 

Dean  of  Science  and  Engineering 


ABSTRACT 


Flexibility  effects  on  robot  manipulator  design  and 
control  are  typically  ignored  which  is  justified  when 
large,  bulky  robotic  mechanisms  are  moved  at  slow  speeds 
However,  when  increased  speed  and  improved  accuracy  is 
desired  in  robot  system  performance  it  is  necessary  to 
consider  flexible  manipulators.  This  project  simulates 
the  motion  of  a  single-link,  flexible  manipulator  using 
the  Equivalent  Rigid  Link  System  dynamic  model  and 
experimentally  validates  the  computer  simulation  results 
Validation  of  the  flexible  manipulator  dynamic  model  is 
necessary  to  ensure  confidence  of  the  model  for  use  in 
future  design  and  control  applications  of  flexible 
manipulators . 
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I.  INTRODUCTION 


The  desire  to  design,  build,  and  operate  a  lightweight, 
long-reach  manipulator  for  NASA  space  shuttle  usage  was  a 
prime  impetus  for  generating  interest  in  the  flexibility 
effects  of  robotic  arms  [Ref.  1:  pp.  3,  9],  Interest  in 
improving  productivity  in  automated  manufacturing  plants 
through  increased  speed  and  accuracy  have  additionally 
sparked  a  desire  to  further  investigate  flexibility  effects 
in  mechanical  devices  [Ref.  2:  pp.  1,  2J.  The  design, 
construction,  and  operation  of  a  flexible  robotic  manipulator 
arm  has  many  attractive  features.  In  the  past  the  manipulator 
was  assumed  being  composed  of  rigid  links  and  dictated  that 
the  link  design  be  large  members,  both  in  cross-sectional 
area  and  weight.  The  flexible  arm  would  require  minimal 
material  and  consequently  would  have  less  weight  and  bulk 
than  conventional  rigid-arm  robots.  With  a  smaller,  lighter 
weight  manipulator,  less  power  would  be  needed  to  move  the 
arm  which  could  mean  use  of  smaller  actuators .  Arm  speed 
movement  would  increase  if  the  actuator  size  is  not  reduced. 

A  smaller,  lighter  weight  manipulator  would  require  less 
foundation  mounting  strength  and  rigidity  requirements.  The 
reduced  foundation  mounting  requirements  coupled  to  the  less 
material  required  for  the  arm  construction  would  translate 
to  a  lower  overall  cost  to  build  a  flexible-arm  robot  compared 
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to  a  rigid-arm  robot.  The  reduced  mass  of  the  flexible-arm 
robot  would  cause  less  damage  if  inadvertent  collision 
occurred  and  would  be  consequently  safer  to  operate  compared 
to  a  rigid-arm  robot.  Finally,  the  reduced  weight  of  the 
flexible-arm  robot  would  allow  for  easy  transportability 
[Ref.  3:  p.  1209]  . 

The  preceding  discussion  of  the  advantages  of  utilizing 
flexible-arm  robots  points  to  their  tremendous  potential  for 
application  in  industry,  in  the  military,  and  in  space.  In 
spite  of  these  advantages  for  utilizing  flexible-arm  robots, 
until  recently  there  has  been  a  reluctance  to  investigate 
the  design  and  control  of  flexible  manipulator  arms.  One 
reason  for  this  reluctance  is  the  degradation  of  the  end- 
effector  positioning  accuracy  due  to  the  increased  deformation 
of  the  lightweight,  flexible  arm.  Also,  the  increased  vibra¬ 
tion  of  the  flexible  arm  causes  a  significant  control  problem 
when  coupled  with  the  large-scale  translational  and/or 
rotational  motion  of  the  robot  [Ref.  3:  p.  1209].  This  control 
problem  arises  due  to  the  reduced  bandwidth  of  the  flexible 
manipulator  system  and  the  consequent  limitation  on  values  of 
gain  in  the  control  design.  The  reduced  bandwidth  is  the 
result  of  the  lower  fundamental  frequency  inherent  in  a 
flexible  manipulator  system  compared  to  a  rigid  system.  In 
order  to  benefit  from  the  advantages  of  lightweight , flexible 
manipulators  it  is  necessary  to  implement  a  control  design 
capable  of  achieving  end-effector  positioning  accuracy  and 


stable  control. 


Extensive  research  began  in  the  early  1980's  into  the 
design  and  control  aspects  of  a  flexible  manipulator  arm. 
Information  on  the  dynamic  response  and  the  natural  frequen¬ 
cies  of  the  flexible  manipulator  arm  is  useful  to  the  designer 
in  predicting  deformations  and  stress  levels.  An  accurate 
dynamic  model  including  flexibility  allows  for  simulation 
studies  by  the  designer  enabling  him  to  extract  his  required 
information.  An  accurate  dynamic  model  including  flexibility 
is  also  necessary  for  any  controller  design  which  is  to  sub¬ 
sequently  control  a  flexible  manipulator  [Ref.  2:  pp.  2,  3]. 

An  integral,  essential  part  of  improving  the  accuracy  and 
stability  problems  associated  with  flexible  manipulator  arms 
is,  therefore,  the  development  of  an  accurate,  dynamic  model 
of  the  flexible  arm.  There  are  several  approaches  to  the 
development  of  a  flexible  structure  dynamic  model  in  the 
literature,  some  of  which  are  reviewed  below. 

Until  recently,  the  approach  to  modeling  robotic  mecha¬ 
nisms  assumed  a  rigid  structure.  The  motion  consequently 
described  by  these  models  included  only  the  large,  rigid- 
body  motion,  hereafter  referred  to  as  large  motion.  The 
recent  approach  in  the  development  of  a  dynamic  model  for 
robotic  mechanisms  is  to  include  the  small  motion  deforma¬ 
tions  arising  from  the  flexibility  of  the  structure, 
hereafter  referred  to  as  small  motion.  These  small  motion 
deformations  include  bending,  twisting,  and  axial  extending. 
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Sunada  and  Dubowsky  [Ref.  4]  utilized  the  4x4  transforma¬ 
tion  matrices  including  the  effects  of  flexibility  to  describe 
the  kinematics  of  flexible  arm  motion,  specifically  applied  to 
industrial  robots.  The  small  motion  deformations  were  super¬ 
imposed  on  an  assumed  nominal  large  motion  to  include  the 
effects  of  flexibility.  This  model  ignored,  however,  the 
effect  of  the  small  motion  interaction  on  the  large  motion, 
and  consequently  did  not  give  a  complete  description  of  the 
actual  motion  dynamics .  Sunada  and  Dubowsky  utilized  finite 
element  method  techniques  and  a  method  to  discretize  the 
distributed  motion  known  as  Component  Mode  Synthesis  to 
obtain  linear,  ordinary,  differential  equations  of  motion. 

Book  [Refs.  5,  6]  similarly  included  the  small  motion 
deformations  in  the  4x4  transformation  matrices  but  he 


ad 
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utilized  a  modal  approach  to  model  the  flexible  kinematics 
and  truncated  the  series  of  assumed  vibration  modes.  After 
application  of  Lagrange's  equation  and  utilizing  a  combined 
set  of  large  and  small  motion  hybrid  coordinates,  a  complica¬ 
ted  set  of  dynamic  equations  of  motion  were  obtained.  The 
resulting  equations  of  motion  were  non-linear  in  both  large 
and  small  motion  variables  and  were  consequently  time- 
consuming  and  expensive  to  solve. 

Cannon  and  Schmitz  [Ref.  7]  utilized  a  similar  modal 
approach  with  a  Lagrangian  formulation  to  model  a  single¬ 
link  flexible  arm.  This  particular  model  was  obviously  very 
restrictive  in  its  application  to  robotic  mechanisms.  Cannon 


I 
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and  Hollars  [Ref.  8]  are  investigating  the  modeling  and 
control  of  two- link  manipulators  with  flexible  tendons, 
but  it  appears  that  the  resultant  dynamic  model  is  likewise 
restrictive  to  this  special  application. 

Truckenbrodt  [Ref.  9]  modeled  the  flexible  manipulator 
as  a  "hybrid  multibody  system"  consisting  of  both  rigid 
and  flexible  elements .  The  application  of  his  model  was  to 
a  specialized  example  and  compatibility  between  links  was 
not  clearly  shown. 

Huston  [Ref.  10 ]  developed  the  dynamic  equations  of 
motion  for  a  flexible,  multi-link  manipulator  utilizing 
a  combination  Newton-Euler  approach  and  d'Alembert's 
principle.  His  assumption  of  a  nominal  large  motion  on 
which  to  apply  the  small  motion  deformations  was  similar 
to  Dubowsky's  and  results  in  an  incomplete  representation 
of  the  actual  motion  dynamics . 

Chang  [Ref.  2]  introduced  an  Equivalent  Rigid  Link 
System  (ERLS)  to  describe  the  large  motion  kinematics. 

The  small  motion  deformations  were  described  relative  to 
the  Equivalent  Rigid  Link  System.  Applying  finite  element 
techniques  and  Lagrangian  dynamics,  two  sets  of  coupled, 
non-linear,  ordinary  differential  equations  of  motion  were 
obtained.  Because  of  the  use  of  the  ERLS,  these  sets  of 
equations  were  composed  of  one  set  for  large  motions  and  one 
set  for  small  motions.  The  set  of  large  motion  equations 
were  non-linear  in  both  the  large  and  small  motion  variables. 
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The  set  of  small  motion  equations  were  linear  in  the  small 
motion  variable  and  non-linear  in  the  large  motion  variable. 
These  particular  characteristics  of  the  resultant  sets  of 
equations  of  motion  allowed  for  their  relatively  easy  solution 
by  a  technique  developed  by  Chang  known  as  the  Sequential 
Integration  Method.  Chang's  model  offered  a  complete  repre¬ 
sentation  of  the  dynamics  of  flexible  manipulators  as  well 
as  one  that  could  be  efficiently  solved  using  the  Sequential 
Integration  Method. 

The  purpose  of  this  research  is  to  experimentally  validate 
the  accuracy  of  a  dynamic  model  including  flexibility.  The 
dynamic  model  chosen  is  that  developed  by  Chang.  This  model 
is  tailored  to  a  single-link  flexible  arm  that  was  designed, 
constructed,  and  operated  for  the  purpose  of  the  dynamic 
model  validation.  Hydraulic  actuation  of  the  single-link 
arm  is  utilized  and  the  motion  of  the  arm  is  limited  to  a 
vertical  plane.  Computer  simulation  of  the  experimental 
flexible  arm  using  the  adapted  dynamic  model  is  on  the  IBM 
3033.  The  integration  methods  from  the  Continuous  System 
Modeling  Program  (CSMP)  are  utilized  in  the  solutions  of  the 
dynamic  equations  of  motion.  Techniques  for  the  acquisition 
of  position  data  of  the  moving  experimental  arm  are  reviewed 
in  a  subsequent  chapter.  Photography  and  strain  gauge 
measurement  prove  to  be  the  most  economical  and  simple 
procedures  in  this  application. 
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The  remainder  of  this  thesis  includes  a  chapter  on  the 
theoretical  approach  to  model  the  dynamics  of  the  flexible 
manipulator  using  the  Equivalent  Rigid  Link  System  (ELRS } . 

The  Theoretical  Approach  chapter  also  includes  a  discussion 
on  the  modelling  of  the  hydraulic  actuation  dynamics  and  a 
discussion  of  the  computer  simulation  of  the  entire  flexible 
manipulator  system. 

A  chapter  is  devoted  to  a  description  of  the  experimental 
approach  utilized  in  validating  the  dynamic  simulation  results 
The  chapter  reviews  the  design  of  the  experimental  manipulator 
system,  including  the  hydraulic  actuation  and  the  flexible  arm 
The  Experimental  Approach  chapter  also  reviews  various  tech¬ 
niques  investigated  for  experimentally  determining  the  end¬ 
point  position  of  the  flexible  manipulator. 

The  Results  chapter  follows  and  presents  a  comparison  of 
the  arm-tip  position  data  obtained  during  the  experimentation 
to  the  arm-tip  position  data  obtained  from  the  computer 
simulation.  Also,  a  comparison  is  made  between  the  actual 
strain  of  the  experimental  arm  and  the  strain  predicted  by 
the  ERLS  dynamic  model. 

A  chapter  devoted  to  the  control  of  flexible  manipulators 
is  included  to  provide  a  brief  literature  review  on  this 
aspect  of  flexible  manipulator  research.  Additionally,  an 
initial  attempt  at  controlling  the  single-link  flexible 
manipulator  using  the  ERLS  model  is  discussed. 


Finally,  a  chapter  each  is  devoted  to  drawing  conclusions 
on  the  lessons  learned  in  this  research  effort  and  for  making 
recommendations  on  the  future  direction  of  research  in 
flexible  manipulators  . 
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II.  THEORETICAL  APPROACH 


Chang's  dynamic  model  is  based  on  his  introduction  of  an 
Equivalent  Rigid  Link  System  (ERLS)  to  describe  the  large 
rigid  motion  of  the  flexible  manipulator  system.  The  small 
motion  deformations  are  described  relative  to  the  ERLS .  The 
local  coordinate  system  for  each  link  is  defined  in  the  ERLS 
and  deformations  are  measured  relative  to  this  coordinate 
system.  Coordinate  transformation  utilizing  joint  variables 
of  the  ERLS  is  applied  to  the  actual  deformed  position  at  any 
point  on  a  link  to  obtain  the  absolute  position  of  that  point. 
Time  derivatives  of  the  absolute  positions  are  necessary  for 
the  kinetic  energy  derivation  for  use  in  Lagrange's  equations. 

It  is  necessary  to  discretize  the  deformations  since 
these  displacements  are  for  each  point  along  the  flexible 
arm.  The  Finite  Element  Method  (FEM)  is  utilized  to  accom¬ 
plish  this  discretization  of  the  deformations.  The  FEM  nodal 
displacements  represent  the  small  motion  deformations  at  the 
end  of  the  link.  A  cubic  shape  function  is  assumed  for  each 
beam  element.  Choice  of  a  cubic  shape  function  ensures  the 
complete  representation  of  the  displacement  including  rigid 
body  rotation,  translation,  and  compatibility  of  the  displace¬ 
ment  between  elements. 

After  having  described  the  kinematical  relationships 
between  the  large  and  small  motions,  kinetics  is  introduced 
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to  complete  the  derivation  of  the  dynamic  equations  of 
motion.  Utilizing  the  Lagrangian  formulation  requires  the 
definition  of  generalized  coordinates.  The  description  of 
large  and  small  motions  are  a  logical  choice  for  the 
generalized,  coordinates.  The  joint  variables  of  the  ERLS 
and  the  nodal  displacements  are  the  two  sets  of  generalized 
coordinates  utilized  in  Lagrange's  equations.  The  kinetic 
energy  has  contributions  from  each  link,  actuators,  and  any 
loading.  The  potential  energy  has  contributions  from  the 
elastic  strain  energy  and  from  gravity.  Generalized  forces 
are  included  due  to  any  applied  forces  and  damping  forces . 
After  considerable  effort  in  mathematical  manipulations, 
rearrangements,  and  simplifications,  the  Lagrange  equations 
yield  two  sets  of  non-linear,  coupled, second-order ,  ordinary 
differential  equations.  One  set  of  equations  describes  the 
large  motions  and  the  other  set  of  equations  describes  the 
small  motions,  though  both  sets  remain  coupled.  Details  of 
the  adaptation  of  Chang's  ERLS  model  to  the  experimental, 
single-link,  flexible  arm  are  contained  in  Appendix  A. 

The  equations  of  motion  for  the  single-link  flexible 
arm  are  written  as  two  sets  of  equations,  one  set  consisting 
of  one  equation  for  the  large  motion  and  one  set  consisting 
of  two  equations  for  the  small  motion  as  follows, 

MQQ  9  +  MQN  U  =  FQ  (1) 

MNQ  9  +  MNN  U  +  KN  U  =  FN  (2) 

where , 


MQQ  is  the  lxl  inertia  matrix  for  large  motions , 

MQN  is  the  1x2  coupled  inertia  matrix  of  the  small  motion 
contribution  to  the  large  motion, 

MNQ  is  the  2x1  coupled  inertia  matrix  of  the  large  motion 
contribution  to  the  small  motion, 

MNN  is  the  2x2  inertia  matrix  for  small  motion, 

KN  is  the  2x2  stiffness  matrix, 

FQ  is  the  lxl  load  vector  for  the  large  motion, 

FN  is  the  2x1  load  vector  for  the  small  motion, 

9  is  the  generalized  coordinate  of  the  single  joint 
variable  representing  the  large  motion, 

U  is  the  2x1  generalized  coordinate  vector  of  the 
deformations  representing  the  small  motion. 

Utilizing  hydraulic  actuation  for  the  single-link 
manipulator  necessitates  the  derivation  and  the  inclusion 
of  the  hydraulic  actuator  dynamics  into  the  equations  of 
motion  of  the  flexible  arm.  The  inclusion  of  the  hydraulic 
power  system  dynamic  equations  into  the  flexible  manipulator 
equations  of  motion  involves  the  transformation  of  an  input 
current  to  an  output  torque.  The  servovalve  and  actuator 
dynamics  are  included  to  make  the  description  complete.  Moog 
simplifies  their  servovalve  dynamics  to  a  single  non-linear 
equation  [Ref.  11], 

Q  =  I  K  y?;  (3) 

where  Q  is  the  flow  delivered  from  the  servo  valve, 

I  is  the  input  current. 


K  is  the  valve  sizing  constant  computed  from  the  flow 
conditions  and  is  the  servovalve  contribution  to  the 
overall  hydraulic  system  damping 

Pv  is  the  valve  pressure  drop  and  is  equal  to  the 
difference  between  the  supply  pressure,  Pg ,  and  the 
load  pressure  drop,  P^. 

The  actuator  dynamic  includes  the  following  continuity 
equation  and  the  torque  output  equation  [Ref.  12,  pp.  133-138 
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where  Q  is  flow  delivered  from  the  servovalve  to  the 
actuator, 

Dm  0  is  the  flow  component  causing  actuator  rotation, 

Ctm  P1  ‘*'s  the  leaJca<?e  flow  in  the  actuator, 

V  P 
t  1 

4  -g  is  the  compressibility  flow. 


is  the  required  torque  delivered  to  move  the  load 
and  to  overcome  intertia, 
nfc  is  the  torque  efficiency, 

Dm  is  the  motor  displacement, 

9  is  the  actuator  motor  angular  velocity, 


P^  is  the  time  derivative  of  the  load  pressure  drop,  P^, 


is  the  total  leakage  coefficient  of  the  actuator 
and  is  the  actuator  contribution  to  the  overall  hydraulic 
system  damping 

V  is  the  total  compressed  volume  including  actuator 
lines  and  chambers, 

3  is  the  effective  bulk  modulus  of  fluid, 
e 

Values  for  each  of  these  parameters  are  computed  from 
the  actuator  specifications  and  from  good  engineering 
judgement. 

The  following  diagram  shows  the  transformation  of  the 
input  current  to  an  output  position  and  includes  the  hydraulic 
and  flexible  manipulator  dynamics. 
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The  preceding  hydraulic  dynamic  equations  and  relation¬ 
ships  are  incorporated  into  the  main  computer  program  for 
solving  the  dynamic  equations  of  motion  for  the  experimental 
single-link  flexible  arm  listed  in  Appendix  B. 

Computer  simulation  of  the  equations  of  motion  for  the 
experimental  arm  and  hydraulic  actuator  required  the  solu¬ 
tion  of  three  simultaneous,  non-linear,  coupled,  second-order, 
ordinary,  differential  equations.  Fortran  language  and  the 
double  precision,  variable-step,  fourth-order,  Runge-Kutta 


integration  method  available  through  the  Continuous  System 
Modeling  Program  (CSMP)  are  utilized  in  coding  the  simulation. 

Eq.  1  and  2  can  be  substituted  into  a  matrix  format  to 
create  a  3x3  coefficient  inertia  matrix  and  a  3x1  right-hand 
side  of  forces  and  moments.  The  unknowns  become  the  large 
motion  joint  variable  acceleration  and  the  small  motion 
deformation  vector  accelerations.  The  matrix  format  appears 
as  follows: 


MQQ 

MQN 

0 

= 

FQ 

MNQ 

MNN 

U 

FN  -  KN  U 

Construction  of  the  computer  coding  involves  forming 
each  of  the  elements  of  the  coefficient  inertia  matrix  and 
the  force/moment  vector  in  a  separate  subroutine.  Once 
formed  the  elements  are  assembled  into  the  matrix  after 
which  an  IMSL  linear  equation  solver  subroutine  is  used  to 
solve  for  the  accelerations.  The  accelerations  are  integrated 
twice  using  the  double  precision,  variable-step,  fourth-order, 
Runge-Kutta  integration  method  available  through  the  Continu¬ 
ous  System  Modeling  Program  (CSMP) .  Finally  a  transformation 
from  local  coordinates  to  global  coordinates  takes  place  to 
get  global  position  information  on  the  motion  of  the  arm  tip. 
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Initially,  the  single-link  parameters  and  the  motion 
variables  initial  conditions  are  input.  There  is  also  a  need 
to  form  the  various  transformation,  inertia,  and  assorted 
other  matrices  for  use  later  in  the  inertia  and  force/moment 
matrix  subroutines.  Another  prerequisite  for  constructing 
the  equations  of  motion  are  subroutines  for  doing  matrix 
multiplication  and  addition,  and  for  doing  the  matrix  opera¬ 
tions  of  the  transpose  and  the  trace.  These  subroutines  are 
all  listed  in  the  copy  of  the  coding  in  Appendix  B. 

Results  from  the  computer  simulation  and  their  comparison 
to  the  actual  experimental  motion  data  are  discussed  in  the 
Results  chapter. 


III.  EXPERIMENTAL  APPROACH 


The  experimental  validation  of  the  Equivalent  Rigid  Link 
System  (ERLS)  dynamic  model  on  a  single-link  flexible  manipu¬ 
lator  required  a  significant  preliminary  design  effort.  The 
power  system  for  the  experimental  arm  needed  to  be  chosen, 
designed,  and  purchased.  Detailed  design  of  the  single-link 
flexible  arm  needed  to  be  completed  and  the  arm  needed  to  be 
manufactured.  Techniques  for  the  measurement  of  the  position 
versus  time  of  the  flexible  arm  tip  needed  to  be  investigated 
and  a  suitable  technique  chosen.  The  power  system  and  the 
experimental  single-link  flexible  arm  finally  needed  to  be 
assembled  into  an  operational  system  and  the  arm  tip  position 
measurement  technique  needed  to  be  implemented. 

It  was  decided  to  choose  a  hydraulic  system  to  power  the 
experimental  arm.  The  reasons  for  this  choice  included  the 
desire  to  increase  the  Mechanical  Engineering  Departments' 
exposure  to  hydraulics  and  to  utilize  the  knowledge  gained 
while  taking  the  Fluid  Power  Control  course.  The  Naval 
Surface  Weapons  Center,  White  Oak,  located  at  Silver  Spring, 
Maryland  agreed  to  fund  the  purchase  of  the  required  components 
of  the  hydraulic  power  system  as  a  result  of  their  support  of 
the  robotic  research  effort  in  the  Mechanical  Engineering 
Department  at  the  Naval  Postgraduate  School. 
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The  design  of  the  hydraulic  power  system  involved  the 
selection  of  the  appropriately  sized  actuator,  servovalve, 
and  power  unit,  as  well  as  the  selection  of  a  suitable  servo- 
controller,  high  pressure  filter  and  position  transducer. 
Miscellaneous  hoses  and  fittings  were  obviously  also  required 
for  the  final  system  assembly. 

The  power  supply  selected  was  a  York  hydraulic  power  unit 
that  was  available  in  the  Mechanical  Engineering  Department. 
This  unit  was  overhauled  and  upgraded  to  include  a  3  horse¬ 
power  motor  and  starter  to  increase  the  system  supply  pressure 
to  2250  psi. 

The  selection  of  the  servovalve  and  actuator  required  the 
following  analysis  [Ref.  12:  pp.  81,  133-138] 

1.  Assume  a  load  description  of  the  form 

J  0  +  TL  =  Td 

where  J  is  the  total  moment  of  inertia  of  the  arm 
and  the  load  reflected  from  the  base, 

T^  is  the  maximum  load  torque  including  the  weight 
of  the  arm  and  maximum  loading  (15  lbf)  in  the  horizontal 
position , 

is  the  required  torque  delivered  to  move  the  load 
and  to  overcome  inertia, 

9  is  the  actuator  motor  angular  acceleration 

2 

2.  Assume  a  design  point  of  9  =  45  deg/sec,  9  =  45  deg/sec 
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3.  The  given  geometry  and  dimensions  of  the  arm  and  loading 
results  in  the  following  total  inertia, 

J  =  74.6942  in-lbf-sec^. 

4.  The  maximum  load  torque  including  the  weight  of  the  arm 
and  the  loading  is, 

T,  =  804.380  in-lbf. 

5.  Assume  the  system  supply  pressure,  P  ,  is  2000  psi  and 
that  the  load  pressure  drop,  P^,  is  2/3  of  Pg  =  1333.3  psi. 
Assume  the  torque  efficiency  is  worst  case,  r>  =  -6. 

The  required  displacement  of  the  motor,  D^,  is 
therefore , 

T 

D  =  — 5-  =  1.0788  in 3/ rad 

m  n  ^ 

The  selected  actuator  must  at  least  have  this  displace¬ 
ment.  Bird-Johnson ' s  3-axis  Hyd-Ro-Wrist  with  a  displacement 
in  the  pitch  axis  of  4.0  in  /rad  was  chosen.  The  wrist 
additionally  has  yaw  and  roll  axes  each  with  a  1.0  in^/rad 
displacement.  However,  in  this  thesis  research  only  the 
pitch  axis  was  utilized  and  the  yaw  and  roll  axes  were 
removed . 

6.  The  selection  of  the  servovalve  required  an  estimation 
of  the  flow  delivered  to  the  actuator  at  design  conditions. 
The  continuity  equation  (Eq.  4)  describes  the  flow  to  the 


At  design  conditions  P1  is  assumed  zero.  After  using 
good  engineering  judgement  in  assuming  values  for  each 
parameter,  the  design  flow  corrected  to  rated  conditions 
becomes , 

Q-design  =  .637  gpm 

Moog  760-100  servovalve  having  a  1.0  gpm  rated  flow 
was  selected. 

7.  A  Moog  servocontroller  and  a  high  pressure  filter 
assembly  were  chosen  to  complete  the  hydraulic  power 
system.  A  Bourn  potentiometer  will  be  used  to  extract 
large  motion  rotation  data  for  use  with  the  photography 
measurements  in  determining  arm  tip  position. 

Ideas  for  the  design  of  the  experimental  arm  were 
initially  investigated  by  visiting  the  robotic  research 
laboratories  at  Stanford  University  and  at  SRI  International. 
The  experimental  flexible  manipulator  systems  utilized  by 
Cannon  at  Stanford  and  by  Andeen  at  SRI  provided  valuable 
information  for  the  design  of  the  experimental  arm  [Ref.  7]. 
Figures  1,  2  and  3  are  photographs  of  the  experimental  flexible 
arm  and  the  hydraulic  power  system.  The  basic  configuration 
of  the  arm  includes  two  parallel, flexible,  steel,  flat  bars 
connected  by  thin  steel  strips  to  transverse  steel  bridges. 

The  parallel  flat  bars  provide  for  flexibility  in  a  vertical 
plane  only.  This  flexibility  is  minimally  hindered  by  the 
transverse  bridges  because  of  the  thin,  connecting,  flexible 


*  *j 


strips.  The  transverse  bridges  increase  torsional  rigidity 
and  reduce  the  tendency  of  the  arm  to  twist  while  in  motion. 
Figure  4  is  a  photograph  showing  the  inclusion  of  the  trans¬ 
verse  bridges  in  the  flexible  arm  construction.  External 
loading  is  attached  to  the  arm  tip  end  transverse  bridge 
by  the  securing  of  the  load  on  the  four  welded  studs.  The 
hydraulic  actuator  is  attached  to  the  other  end  of  the 
flexible  arm. 

The  validation  of  the  ERLS  dynamic  model  requires  the 
comparison  between  the  predicted  arm  tip  position  from  the 
model  and  the  actual  arm  tip  position  from  the  experimental 
single-link  manipulator.  The  computer  program  listed  in 
Appendix  B  generates  the  flexible  arm  tip  position  referenced 
to  a  planar,  global  coordinate  system  having  the  origin  at 
the  hydraulic  actuator  rotation  axis.  The  problem  of  deter¬ 
mining  the  arm  tip  position  of  an  experimental,  flexible 
manipulator  is  far  more  difficult  than  that  experienced  in 
determining  the  arm  tip  position  of  a  rigid  manipulator. 

There  is  currently  a  significant  research  effort  in  developing 
accurate  techniques  for  arm  tip  position  measurement  and 
control  of  a  flexible  manipulator.  A  brief  summary  of  the 
techniques  investigated  for  possible  use  in  this  thesis 
research  follows. 

The  first  technique  considered  was  the  crude  but  effec¬ 
tive  method  of  taking  motion  pictures  of  the  arm  against  a 
grid  background  with  a  time  counter  in  the  field  of  view. 


The  arm  tip  position  at  any  time  is  manually  determined  on 
each  picture  frame  by  counting  the  number  of  grid  lines  in 
the  vertical  and  horizontal  directions. 

Cannon  and  Schmitz  [Ref.  7]  utilized  an  optical  sensing 
system  of  a  focusing  lens,  a  photodetector,  an  amplifier, 
and  an  A/D  converter  to  acquire  position  data  from  their 
horizontally  moving,  flexible  arm.  An  incandescent  light 
bulb  was  affixed  to  the  tip  of  the  arm  and  provided  the 
light  intensity  that  was  received  by  the  optical  sensing 
system.  This  technique  appeared  to  be  suitable  for  its 
specialized  application,  but  may  not  be  as  satisfactory  in 
general  usage. 

The  National  Bureau  of  Standards  has  conducted  manipula¬ 
tor  end-point  position  sensing  experiments  using  an  automatic 
laser  tracking  interferometer  system.  Initial  experiments 
have  provided  very  promising  results  [Ref.  13].  The  signifi¬ 
cant  drawbacks  to  this  laser  tracking  technique  were  its 
current  high  cost  and  complicated  technology. 

Andeen  has  utilized  strain  gages  and  extrapolated  the 
deflection  to  the  arm  tip  assuming  first  mode  vibration. 

This  technique  was  relatively  simple,  but  may  be  inaccurate 
unless  the  predominant  mode  of  vibration  is  the  first  mode. 


Interfacing  the  planar  motion  of  the  flexible  arm  to  a 
digitizing  tablet  provided  a  potentially  feasible  technique 
for  acquiring  arm  tip  position.  This  technique  would  allow 
for  automatic,  time-efficient,  position  data  acquisition. 


This  technique  was  obviously  only  applicable  to  planar 
motion  of  the  manipulator  and  was  expensive  to  implement 
for  large  motion  excursions  of  the  arm. 

Lt.  William  M.  Dunkin  [Ref.  14]  conducted  research  at 
the  Naval  Postgraduate  School  on  the  use  of  ultrasonics  for 
a  position  reference  system  of  a  manipulator  arm.  His  work 
was  preliminary  in  nature  and  his  experiments  utilized  a 
stationary  manipulator.  Despite  the  poor  accuracy  of  the 
results,  use  of  ultrasonics  for  arm  tip  positioning  and 
control  has  significant  potential  if  further  research 
continues  to  perfect  this  technique. 

Use  of  a  position/displacement  transducer  that  provided 
an  electrical  signal  proportional  to  the  linear  extension  of 
a  cable  offered  another  technique  for  automatic,  position 
data  acquisition.  Arranging  a  transducer  on  each  planar 
coordinate  axis  and  affixing  each  cable  to  the  arm  tip  would 
allow  for  accurate  positioning  to  occur.  To  utilize  this 
measuring  technique  would  require  the  inclusion  of  the  cable 
tension  in  the  arm  dynamic  equations  of  motion. 

Use  of  accelerometers  followed  by  two  electrical  integra¬ 
tions  to  yield  position  information  offers  good  frequency 
response  and  are  commercially  available.  Use  of  a  digitizing 
vision  system  for  automatic  position  data  acquisition  has 
great  promise  for  future  robotic  app  ications,  but  is 
currently  very  expensive  to  implement.  [Ref.  15 J 


For  this  research  the  use  of  motion  pictures  and  strain 
gages  were  selected  based  on  the  availability,  simplicity, 


and  cost.  The  use  of  motion  pictures  in  planar  applications, 
though  tedious,  can  provide  excellent  results  of  arm  tip 
position  data.  Strain  gages  are  utilized  to  compare  the 
actual  strain  of  the  experimental  arm  to  the  strain  predicted 
by  the  ERLS  dynamic  model. 


IV.  RESULTS 


The  validation  of  the  ERLS  dynamic  model  includes  a 
comparison  between  the  actual  arm  tip  position  and  the 
predicted  arm  tip  position.  The  actual  arm  tip  position 
measurements  were  obtained  by  taking  motion  pictures  of 
the  experimental  arm  and  by  a  frame-by-frame  examination 
of  the  motion.  As  an  additional  check,  the  utilization 
of  strain  gages  allowed  for  a  comparison  between  the  actual 
strain  of  the  experimental  arm  and  the  strain  predicted  by 
the  ERLS  dynamic  model. 

Evaluation  of  the  plots  of  experimental  and  theoretical 
position  or  strain  requires  a  comparison  criteria.  Frequency 
and  amplitude  are  the  parameters  utilized  in  establishing 
the  criteria  for  comparison.  Similarity  in  frequency  content 
and  amplitude  is  necessary  for  determination  of  proper 
control  action  in  the  closed-loop  servo  system  design  and 
for  an  accurate  representation  of  the  actual  motion  in  any 
flexible  manipulator  machinery  design  application.  A  relative 
percentage  error  of  +/-10%  from  the  experimental  results  is 
considered  the  standard  for  comparison.  The  strain  amplitude 
and  frequency  errors  are  computed  by  taking  the  difference 
between  the  theoretical  and  experimental  strain  values.  The 
strain  amplitude  is  a  combination  of  both  fundamental  and 
second  mode  amplitudes  but  examination  of  the  plots  reveals 


that  the  first  mode  amplitude  is  dominant.  The  strain 
frequency  error  is  separated  into  the  fundamental  and 
second  mode  errors.  The  tip  position  amplitude  error  is 
computed  by  taking  the  square  root  of  the  sum  of  the  squares 
of  the  differences  between  the  theoretical  and  experimental 
X  and  Y  coordinate  positions.  The  tip  position  frequency 
errors  are  computed  by  taking  the  difference  between  the 
theoretical  and  experimental  tip  position  frequencies.  Only 
the  first  mode  frequency  and  amplitude  errors  are  determined 
for  the  arm  tip  position.  The  normalization  of  the  absolute 
error  to  a  relative  error  is  accomplished  using  the  arm 
length  for  the  tip  position  amplitude  measurements  and  the 
experimental  strain  amplitude  for  the  strain  amplitude 
measurements.  The  experimental  frequencies  are  used  for 
normalizing  the  tip  position  and  strain  frequency  errors. 

This  normalization  is  accomplished  in  order  to  compute  the 
appropriate  order  of  magnitude  error  between  the  theoretical 
predictions  and  the  experimental  results. 

Figures  6,  7,  and  8  are  plots  for  three  loading  conditions 
of  the  comparison  of  experimental  to  theoretical  arm  tip 
positions  in  the  global  X  (horizontal)  and  Y  (vertical) 
coordinate  directions.  The  three  loading  conditions  are  the 
no-load  condition,  the  5  pound  load  condition,  and  the  10 
pound  load  condition.  The  excitation  of  the  hydraulic 
actuation  for  all  three  loading  conditions  is  a  step  input 
of  4  milliamps  current.  The  initial  condition  for  these 
experimental  runs  is  the  horizontal  position  of  the  flexible 


arm.  Experimental  arm  tip  position  data  requires  a  prelimi¬ 
nary  parallax  correction  and  a  geometric  transformation 
prior  to  plotting.  The  parallax  correction  is  necessary 
because  of  the  relatively  close  proximity  of  the  camera  to 
the  experimental  arm  motion.  The  geometric  transformation  is 
necessary  because  of  the  offset  of  the  base  of  the  experi¬ 
mental  arm  from  the  axis  of  rotation  of  the  hydraulic  actuator. 

Four  CEA-06-12UW-350  strain  gages  are  installed  on  the 
flexible  arm.  Two  are  placed  on  opposite  sides  of  the 
neutral  axis  at  the  base  and  at  the  mid-longitudinal 
position  of  the  arm.  Consequently,  two  gages  provide 
tensile  strain  readings  and  two  gages  provide  compressive 
strain  readings.  A  mid-longitudinal  position  gage  is 
selected  for  plotting  strain  data  because  of  the  higher 
sensitivity,  and  consequently  better  resolution,  in  the 
strip  chart  recording.  The  theoretical  strain  predicted 
by  the  ERLS  dynamic  model  in  the  mid-longitudinal  position 
is  computed  from  the  Finite  Element  shape  matrix  describing 
the  transverse  or  bending  displacement  of  the  flexible  arm. 

The  derivation  of  the  theoretical  strain  is  included  in 
Appendix  C.  Figures  9,  10  and  11  are  plots  for  the  three 
loading  conditions  of  the  comparison  of  experimental  to 
theoretical  microstrain.  Excitation  of  the  hydraulic 
actuation  for  ail  three  loading  conditions  is  a  step  input 
of  4  mi 11 lamps  current.  The  i n i t  i a  1  condition  for  these 
experimental  runs  is  the  vertical  posit  ion  o:  the  flexible 
arm. 
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Figure  9.  Microstrain  For  No  Load. 
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Figure  10.  Microstrain  for  2.115  Kg  Load. 
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Figure  11.  Microstrain  For  4.23  Kg  Load. 


Utilization  of  the  evaluation  criteria  of  +/-10%  error 
reveals  the  tip  position  amplitude  error  is  acceptable.  This 
is  significant  considering  the  importance  of  tip  position 
accuracy  as  a  criteria  for  evaluating  robot  performance. 
Because  of  the  lack  of  adequate  tip  position  accuracy  for 
certain  applications  a  significant  research  effort  is  ongoing 
to  develop  appropriate  tip  sensors  to  compensate  for  the 
position  errors.  The  acceptable  tip  position  amplitude 


error  despite  the  single  element  modelling  allows  for 


relatively  accurate  predictions  of  tip  position  motion  and 
consequently  suggests  the  potential  usefulness  of  the  ERLS 
model  in  improving  the  tip  position  accuracy.  Table  I  lists 


the  relative  percentage  errors  of  tip  position  amplitude 
for  the  different  loading  conditions  given  a  4.0  milliamp 
input  current  to  the  servovalve. 

TABLE  I 

RELATIVE  PERCENTAGE  ERRORS  OF  TIP  POSITION  AMPLITUDE 


No  Load 

10.7 

2.11  Kg  Load 

10.5 

4.23  Kg  Load 

11.5 

The  differences  in  amplitude  observed  in  the  arm  tip 
position  measurements  are  attributed  to  the  error  in 
recording  the  experimental  position  data,  to  the  single 
element  modelling  of  the  experimental  arm,  and  to  the  small 
displacement  assumption  of  the  vibration.  Specifically,  the 
frame-by-frame  examination  of  the  arm  tip  position  is 
hindered  by  the  lack  of  clarity  of  the  arm  tip  and  by  the 
absence  of  definition  of  the  background  grid  measurement 
lines.  Improvement  in  the  grid  spacing  and  color  intensity 
and  in  the  camera  exposure  setting  would  improve  the  quality 
of  the  recorded  data.  The  increased  rigidity  resulting  from 
the  single  element  model  of  the  experimental  arm  is  responsi 
ble  for  the  amplitude  of  the  theoretical  X  tip  and  Y  tip 
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position  data  to  be  less  than  the  experimental  position  data 
and  would  increase  as  the  number  of  elements  is  increased 


in  the  model. 

Axial  deformations  are  neglected  and  small  bending 
displacements  are  assumed  in  the  theoretical  modelling  of 
the  experimental  arm.  The  effect  of  these  assumptions  is 
noticed  in  the  comparison  of  the  X  coordinate  arm-tip  posi¬ 
tion  during  the  first  few  tenths  of  a  second  of  motion. 

The  experimental  arm  tip  position  actually  decreases  during 
this  initial  time  period.  The  theoretical  model  predicts 
an  increase  in  arm  tip  position,  particularly  during  the 
heavier  loading  conditions.  Figure  12  illustrates  how  the 
theoretical  model  could  predict  an  increase  in  the  X  coordi¬ 
nate  tip  position.  The  theoretical  arm  position  approximates 
the  actual  arm  position  and  because  the  small  motion  displace 
ment  is  measured  with  respect  to  the  ERLS  local  coordinate 
axis  the  theoretical  arm  Len  ;th  appears  to  increase.  This 
arm  length  increase  is  espec:  illy  implified  with  large 
displacements  which  resu  It  :  :  n  :.♦•»  ivie:  loading.  For  smaller 
displacements  the  arm  Len  ;tn  i:.  mease  :  s  negligible.  The 
increase  in  the  arm  len jth  is  ret  l  echo  :  by  an  increase  in 
the  theoretical  X  coordinate  t ::  : os i t ion . 

The  motion  pictures  were  taken  at  a  camera  speed  of  24 
frames  per  second.  Since  the  fundamental  and  second  mode 
frequencies  of  the  experimental  arm  without  any  load  are  2 
hertz  and  13  hertz  respectively,  only  the  fundamental 
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frequency  is  observed  by  the  motion  pictures.  Comparison  of 
the  frequency  content  of  the  experimental  and  theoretical 
arm  tip  position  reveals  close  agreement  for  the  fundamental 
frequency.  Comparison  of  the  frequency  content  of  the  experi¬ 
mental  and  theoretical  strain  reveals  that  the  experimental 
results  have  lower  frequency  content  compared  to  the  theoreti¬ 
cal  results  in  both  the  fundamental  and  second  mode  frequencies. 
Utilization  of  the  evaluation  criteria  reveals  the  standard  of 
+/  — 10 %  relative  percentage  error  is  exceeded  for  the  second 
mode  frequency  using  the  strain  measurements.  Table  II  lists 
the  relative  percent  ge  errors  of  the  frequencies  for  the 
different  loading  conditions  given  a  4.0  milliamp  input 
current  to  the  servovalve. 


TABLE  II 

RELATIVE  PERCENTAGE  ERROR  OF  FREQUENCIES 

Strain  Data  Fundamental  Second  Mode 


No  Load 

5 

1 

38 

2.115  Kg  Load 

5 

26 

4.23  Kg  Load 

6 

22  J 

Tip  Position  Data _ Fundamental 


No  Load 

5  | 

2.115  Kg  Load 

5 

4.3  Kg  Load 

5 

The  no-load  theoretical  fundamental  and  second  mode 
frequencies  are  predicted  to  be  2  hertz  and  18  hertz 
respectively.  The  difference  in  frequency  content  between 
the  theoretical  predictions  and  the  experimental  results  is 
explained  in  that  only  one  element  from  the  Finite  Element 
Method  is  utilized  in  modelling  the  experimental  arm.  The 
single  element  model  limits  the  number  of  degrees  of  freedom, 
and  consequently  the  flexibility,  of  the  dynamic  model.  The 
dynamic  model  therefore  appears  more  rigid  than  the  actual 
experimental  arm.  Increasing  the  number  of  elements  in 
modelling  the  experimental  arm  would  increase  the  flexibility 
of  the  dynamic  model  which  would  reduce  the  frequencies  of 
predicted  motion.  Error  between  the  experimental  and  theoreti¬ 
cal  results  is  additionally  introduced  by  the  limited  resolu¬ 
tion  of  the  strain  measurements  from  the  strip  chart  recorder. 

Utilization  of  the  evaluation  criteria  reveals  the 
standard  of  +/-10%  error  is  exceeded  for  strain  amplitude 
measurements.  Table  III  lists  the  relative  percentage  errors 
of  strain  amplitude  for  the  different  loading  conditions 
given  a  4.0  milliamp  input  current  to  the  servovalve.  The 
amplitude  of  the  theoretical  strain  measurements  are  typically 
less  than  the  experimental  strain  measurements  in  both  the 
fundamental  and  second  mode  frequencies.  This  observation  is 
again  explained  by  the  limitation  on  flexibility  imposed  by 
the  single  element  model  of  the  experimental  arm.  Increasing 
the  number  of  elements  in  modelling  the  experimental  arm  would 


increase  the  flexibility  of  the  dynamic  model  which  would 
result  in  increased  amplitudes  of  predicted  strain. 

TABLE  III 

RELATIVE  PERCENTAGE  ERRORS  OF  STRAIN  AMPLITUDE 


Strain  Data 


Table  II  indicates  that  the  relative  percentage  error  of 
the  first  mode  frequency  increases  slightly  while  the  error 
of  the  second  mode  frequency  decreases  as  the  loading  is 
applied.  Tables  I  and  III  indicates  that  the  relative 
percentage  error  of  the  tip  position  and  strain  amplitudes 
increases  slightly  as  the  loading  is  applied.  As  mentioned 
before,  the  first  mode  amplitude  is  dominant.  With  increased 
loading  the  experimental  arm  becomes  softer  or  more  flexible. 

The  dominant  first  mode  strain  amplitude  and  the  first  mode 
tip  position  amplitude  are  slightly  more  difficult  to  predict 
as  the  arm  flexibility  increases.  This  observation  is  con¬ 
sistent  with  the  slight  increase  in  error  in  the  first  mode 
frequency  as  the  loading  increases.  These  trends  are  consistent 
with  the  previous  observations  that  the  theoretical  predictions 
result  in  a  stiffer  system  compared  to  the  experimental  results. 


In  other  words,  the  single  element  model  better  predicts  the 
first  mode  amplitude  and  frequency  of  a  stiff  system  compared 
to  a  softer  system.  As  the  loading  increases  resulting  in 
a  more  flexible  system,  the  first  mode  strain  frequency  and 
dominant  first  mode  strain  amplitude  errors  increase  slightly. 
Similarly,  the  first  mode  tip  position  amplitude  error 
increases  slightly. 

The  trend  of  the  strain  second  mode  frequency  error 
initially  appears  as  an  anomaly  since  the  error  decreases  as 
the  loading  increases.  This  trend  contradicts  the  expected 
result  that  is  observed  in  the  first  mode  frequency  and 
amplitude  error  trends.  The  trend  in  the  second  mode 
frequency  error  suggests  that  the  theoretical  model  more 
easily  predicts  the  second  mode  frequency  of  a  softer  system. 
In  other  words,  the  theoretical  model  is  better  suited  for 
predicting  the  second  mode  frequency  in  a  flexible  system. 

The  accuracy  of  the  theoretical  model  to  predict  the 
experimental  arm  deformation  is  dependent  upon  the  shape 
function  approximation  of  the  natural  modes.  The  choice 
of  the  shape  function  described  in  Appendix  C  to  describe 
the  deformations  results  in  the  observed  trend  in  the  strain 
second  mode  frequency  error  with  loading. 

Investigation  of  the  predicted  and  actual  strains  for 
an  excitation  of  3.0  milliamps  current  to  the  servovalve  was 
made  and  revealed  similar  trends  in  frequency  and  amplitude 
errors  as  noted  in  the  4.0  milliamps  case.  As  expected,  the 


maximum  strains  achieved  and  predicted  are  less  in  the  3.0 
milliamps  case.  However,  the  relative  error  percentages  in 
the  3.0  milliamps  case  are  not  any  less  than  the  4.0  milliamps 
case.  This  suggests  that  the  small  displacement  assumption 
has  little  effect  on  increasing  the  strain  amplitude  and 
frequency  errors  as  the  strain  is  increased.  However,  this 
suggestion  is  obviously  limited  to  this  particular  experiment 
and  needs  to  be  investigated  for  other  values  of  excitation 
current. 

The  assumptions  of  single  element  modelling,  of  small 
displacement  theory,  and  of  no  axial  deformation  are  made 
for  analytical  expediency  and  for  computational  efficiency. 

The  validity  for  making  these  assumptions  should  be  reviewed 
in  light  of  these  experimental  results. 

The  importance  of  correct  modelling  of  the  hydraulic 
dynamics  was  emphasized  when  the  interaction  of  the  hydraulic 
actuation,  the  gravitational  force,  and  the  arm  movement  from 
the  vertical  plane  created  a  serious  resonance  problem.  This 
phenomenom  was  observed  during  the  theoretical  strain  simula¬ 
tions  and  resulted  in  serious  instability  after  approximately 
one  second  of  motion.  Investigation  revealed  that  the  hydrau¬ 
lic  damping  was  improperly  modeled.  The  resonance  was 
eliminated  after  a  modification  to  the  damping  was  made. 
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V.  CONTROL  OF  FLEXIBLE  MANIPULATORS 

The  advantages  to  employing  flexible  manipulators  is 
well  documented  in  the  literature  [Ref.  16:  pp.  101,  102]. 
However,  flexible  manipulator  usage  in  industry  has  been 
minimal  principally  as  a  result  of  the  difficulty  to  control 
the  flexible  manipulator  end-effector  [Ref.  3:  p.  1209] 

There  has  been  considerable  research  recently  in  the 
development  of  flexible  manipulator  control  strategies 
using  state-space  model  techniques.  A  brief  survey  of  this 
research  follows. 

Cannon  and  Schmitz  [Ref.  1]  introduced  the  concept  of 
end-point  position  feedback  for  use  in  controlling  flexible 
manipulators.  The  end-effector  position  was  sensed  and  was 
fed  back  to  the  controller  for  subsequent  determination  of 
the  control  action  required  by  the  joint  actuators.  Use  of 
end-point  position  feedback  would  increase  the  response  speed 
and  would  allow  for  the  use  of  lightweight  flexible  manipula¬ 
tors.  Techniques  for  determining  the  position  of  the 
manipulator  end-point  were  reviewed  in  the  Experimental 
Approach  chapter. 

Cannon  and  Schmitz  [Ref.  7j  utilized  a  modal  approach 
with  a  Lagrangian  formulation  to  model  a  single-link  flexible 
arm.  Both  large  motion  rotation  and  small  motion  deformations 
of  the  flexible  arm  were  included  in  a  single  variable  in  the 


formulation.  The  state-space  model  resulted  in  a  set  of 
decoupled  differential  equations  where  the  states  included 
the  rigid  body  mode  and  the  flexible  modes.  Cannon  and 
Schmitz  limited  the  state-space  model  to  include  only  the 
first  three  flexible  modes.  The  flexible  mode  states 
included  contributions  from  large  motion  rotation  as  well 
as  small  motion  deformations.  The  joint  actuator  provided 
direct  control  action  to  the  large  motion  rotation  of  the 
arm.  Since  the  flexible  mode  states  included  coupling 
between  the  large  and  small  motions,  the  joint  actuator 
provided  control  action  to  the  small  motion  deformations  as 
well.  This  ensured  state  controllability  of  the  state  space 
model.  Output  controllability  was  ensured  after  determination 
of  the  arm-tip  sensor  and  the  joint-rate  sensor  measurement 
vectors  in  the  state-space  output  equation.  The  joint  angle 
and  rate  were  measured  with  a  potentiometer  and  a  tachometer, 
respectively.  Since  all  flexible  mode  states  were  not 
measurable,  Cannon  and  Schmitz  included  an  estimator  in  the 
feedback  control  system  to  ensure  that  the  system  was  observa¬ 
ble.  The  Linear  Quadratic  Gaussian  (LQG)  approach  was 
utilized  in  the  controller  design.  Experimental  verification 
of  the  feedback  control  system  on  a  single-link  flexible 
manipulator  demonstrated  that  stable  and  precise  position 
control  of  the  end-effector  was  achieveable. 

Book  and  Hastings  |Ref.  5]  similarly  utilized  a  Linear 
Quadratic  Regulator  approach  in  designing  a  controller  for  a 


flexible  manipulator.  Their  state-space  model  consisted  of 
a  rigid  body  mode  state  and  the  first  two  flexible  modes  as 
subsequent  states.  Their  initial  formulation  of  the  dynamic 
equations  of  motion  included  coupling  between  the  rigid 
body  mode  and  the  flexible  modes,  and  ensured  state  control¬ 
lability.  Output  controllability  was  ensured  after  determina¬ 
tion  of  the  strain  gauge  sensor,  joint-position  sensor,  and 
joint-rate  sensor  measurement  vectors  in  the  state-space 
output  equation.  The  modal  deflections  were  measured  from 
strain  gauge  data.  Observability  was  ensured  by  including 
an  estimator  in  the  feedback  control  system  to  estimate  the 
two  unmeasured  modal  velocities.  Minimizing  the  first  two 
open-loop  modal  resonances  in  an  experimental  single-link 
flexible  manipulator  confirmed  the  feedback  control  system's 
ability  to  control  the  flexible  modes.  One  significant 
difference  between  the  Book  and  Hastings  model  and  the  Cannon 
and  Schmitz  model  was  that  the  former  utilized  flexible  modes 
corresponding  to  fixed-free  beam  vibrations  whereas  the  latter 
utilized  pinned-free  beam  vibrations.  The  fixed-free  flexible 
mode  model  allows  for  a  more  accurate  extension  to  the  multi¬ 
link  manipulator  since  fixed  link  boundary  conditions  describe 
the  multi-link  physical  system. 

Adaptation  of  Chang's  Equivalent  Rigid  Link  System  flexible 
model  [Ref.  2]  to  the  single-link  manipulator  provided  another 
alternative  to  the  modal  approach  in  defining  state  variables. 
Defining  both  large  motion  rotations  and  small  motion  deforma¬ 
tions  as  generalized  coordinates  in  the  Lagrangian  formulation 
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of  the  dynamic  equations  provided  an  easy  extension  for 
these  coordinates  to  become  state  variables.  A  comparison 
of  the  state-space  model  to  the  ERLS  non-linear  model  was 
needed  to  determine  the  range  of  the  applicability  of  the 
linearized  model  away  from  the  operating  point.  Comparison 
of  simulations  of  open  loop  large  motion  rotation,  theta, 
and  small  motion  deformations  indicated  reasonable  agreement 
between  the  state-space  model  and  the  ERLS  non-linear  model 
for  approximately  1.5  sec.  or  120  degrees  after  an  input 
torque  of  5  N-m  was  applied. 

The  state  space  representation  provided  the  input  to 
the  NPS  mainframe  optimal  feedback  controls  program  CONTROLS, 
subprogram  OPTSYS,  to  design  an  LQR  optimal  controller. 
Coupling  between  the  large  and  small  motions  included  in 
the  Lagrangian  dynamic  equation  formulation  ensured  state 
controllability.  After  assigning  arbitrary  identity  matrices 
for  the  output  measurement  matrix  and  the  weighting  matrices 
for  the  quadratic  performance  index,  an  optimal  feedback 
gain  control  matrix  was  computed  by  the  OPTSYS  program. 
Details  of  the  linearization  and  state-space  representation 
of  the  ERLS  model  and  the  optimal  feedback  control  design 
are  included  in  Appendix  D.  The  definition  of  the  output 
measurement  matrix  assumed  the  feedback  loop  was  closed 
utilizing  tip  control.  Simulation  of  the  state-space  model 
closed-loop  response  to  an  initial  condition  of  a  20  degree 
rotation  away  from  the  zero  degree  operating  point,  a  -.03 


meter  tip  deflection  and  a  -.1  tip  slope  confirmed  the 
linear  feedback  control  systems’  ability  to  control  the 
state-space  model  of  the  single-link  flexible  arm.  A  graph 
of  the  linear  system  closed-loop  response  of  the  large  motion 
rotation,  theta,  is  plotted  in  Figure  13.  This  result  gives 
some  confidence  in  the  linear  controller's  ability  to  control 
the  non-linear  model  given  small  perturbations  about  the 
operating  point.  However,  simulation  of  the  linear  controller 
with  the  non-linear  model  is  necessary  to  investigate  the 
range  of  operation. 

Utilizing  the  arm-tip  deflection  and  slope  as  state 
variables  appears  to  be  an  improvement  over  the  flexible 
mode  state  variables  since  the  former  are  more  easily 
measurable  quantities.  The  need  for  an  estimator  in -the 
feedback  control  system  may  be  eliminated.  However,  addi¬ 
tional  comparison  and  investigation  of  the  merits  of  both 
approaches  are  necessary.  From  the  results  obtained  from 
other  research  it  appears  that  control  of  a  single- link 
flexible  manipulator  is  realizable.  Extension  of  these 
feedback  control  system  approaches  to  multi-link  flexible 
manipulators  is  necessary  if  the  advantages  of  flexible 
manipulators  is  to  be  realized  in  practical  industrial 
applications . 
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VI.  CONCLUSIONS 


The  purpose  of  this  research  is  to  experimentally 
validate  the  ERLS  dynamic  model.  The  validation  of  the 
ERLS  dynamic  model  is  needed  to  ensure  confidence  of  the 
model  for  use  in  future  design  and  control  applications. 

In  this  research,  the  ERLS  model  is  tailored  to  a  single¬ 
link  flexible  arm  having  hydraulic  actuation  and  moving  in 
a  vertical  plane.  The  vertical  plane  motion  introduces 
the  effects  of  gravity.  The  investigation  of  the  effects 
of  gravity  on  flexible  manipulator  movement  allows  for  the 
consideration  of  applications  not  limited  to  space  usage. 

The  investigation  of  hydraulic  power  actuation  allows  for 
the  consideration  of  heavy  load  applications.  The  effects 
and  interactions  of  modelling  the  flexible  arm  with  gravity 
and  the  hydraulic  actuation  revealed  the  importance  of 
proper  determination  of  parameters ,  specifically  those 
affecting  damping. 

The  acceptable  tip  position  amplitude  error  despite  the 
single  element  modelling  suggests  the  ERLS  model's  potential 
usefulness  in  improving  the  tip  position  accuracy.  This 
potential  benefit  is  significant  considering  the  importance 
of  tip  position  accuracy  as  a  criteria  for  evaluating  robot 
performance. 
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The  results  of  the  validation  indicated  the  theoretical 
strain  and  position  measurements  are  affected  by  the  under¬ 
lying  assumptions  of  the  ERLS  model.  Specifically,  the  FEM 
single  element  modelling  of  the  experimental  arm  results  in 
a  more  rigid  description  of  the  actual  motion  and  gives 
smaller  amplitudes  and  higher  frequencies.  The  results 
indicate  that  as  loading  is  applied  to  the  single  element 
model  the  relative  percentage  errors  of  the  first  mode 
amplitude  and  frequency  increase  slightly.  As  loading  is 
applied,  the  experimental  arm  becomes  more  flexible  and  the 
single  element  model's  performance  in  predicting  the  first 
mode  motion  degrades.  The  results  indicate  though  that  the 
single  element  model  is  better  suited  for  predicting  the 
second  mode  frequency  as  the  loading  is  increased.  The 
relative  percentage  errors  for  frequency  indicate,  however, 
that  the  single  element  model  better  predicts  the  first  mode 
motion  compared  to  the  second  mode  motion. 

The  small  displacement  assumption  results  in  additional 
error  to  the  theoretical  strain  and  position  predictions. 

This  error  is  most  noticeable  during  the  initial  stages  of 
the  X  coordinate  tip  position  motion  and  increases  as  the 
loading  increases.  The  small  displacement  assumption  does 
not  appear  to  have  much  effect  though, on  the  strain  amplitude 
and  frequency  errors  as  evidenced  by  the  lack  of  any  error 
increase  as  the  strain  is  increased  by  a  larger  hydraulic 
excitation  current.  This  conclusion  needs  to  be  investigated 


for  other  excitation  current  values.  Other  assumptions 
are  made  on  the  values  of  certain  mechanical  and  hydraulic 
parameters,  particularly  inertia  properties  of  the  arm  and 
actuator.  These  assumed  values  undoubtedly  contribute  to 
the  error  in  the  theoretical  predictions.  The  agreements 
and  differences  between  the  simulation  and  the  experiment 
in  both  arm-tip  position  and  strain  measurements  provide  a 
valuable  validation  of  the  ERLS  model.  The  experimental 
data  serves  as  a  guideline  to  upgrade  the  dynamic  model, 
particularly  in  the  validity  of  the  underlying  assumptions. 
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VII.  RECOMMENDATIONS 


A  principal  goal  in  this  research  is  to  eventually  use 
the  ERLS  dynamic  model  in  the  design  of  a  complete  flexible 
manipulator  system.  This  system  would  include  a  multi-link 
flexible  manipulator  and  a  servo  control  loop.  Two  areas  of 
research  needed  to  achieve  this  goal  are,  therefore,  the 
control  system  design  and  the  optimal  design  of  a  flexible 
manipulator. 

Continued  simulation  studies  of  a  closed-loop  system 
having  the  controller  design  based  on  the  ERLS  model  are 
needed.  Alternative  control  laws  need  to  be  investigated 
for  possible  use  in  this  application.  Concurrent  work  is 
needed  on  the  continued  validation  of  the  ERLS  dynamic  model. 
Specifically,  the  single  element  FEM  modelling  of  deformations 
should  be  extended  to  a  multi-element  model.  Validation  of 
the  arm  and  actuator  inertia  properties  needs  to  be  accom¬ 
plished.  The  intent  of  the  continued  validation  of  the  ERLS 
dynamic  model  is  to  bring  the  theoretical  arm  motion  into 
closer  agreement  to  the  experimental  arm  motion.  This 
refinement  of  the  dynamic  model  is  useful  for  an  effective 
controller  design  based  on  the  model.  Additional  techniques 
for  the  acquisition  of  arm  tip  position  need  to  be  investiga¬ 
ted  and  implemented.  Specifically,  sensors  for  arm  tip 
position  need  to  be  implemented  in  order  to  feedback  position 


data  to  the  controller  for  appropriate  control  action. 
Alternatives  for  arm  tip  position  sensors  include  accelero¬ 
meters  and  optics.  The  controller  design  eventually  needs 
to  be  implemented  in  hardware  and/or  software  and  tested. 
Extension  of  the  controller  design  and  implementation  to 
the  multi-link  case  is  eventually  needed  if  the  advantages 
of  flexible  manipulators  is  to  be  realized  in  practical 
industrial  applications. 

Once  experimental  validation  is  completed  the  ERLS 
dynamic  model  will  allow  computer  simulation  for  designing 
a  mechanical  manipulator  with  a  desired  rigidity.  Further 
investigation  is  needed  into  the  optimal  design  of  a  flexible 
manipulator . 


APPENDIX  A 


DERIVATION  OF  THE  EQUATIONS  OF  MOTION  FOR  THE 
EXPERIMENTAL,  SINGLE -LINK,  FLEXIBLE  ARM 

Given  the  large  and  small  motions  as  generalized  coordi¬ 
nates,  the  following  are  the  two  sets  of  Lagrange  equations 
used  to  develop  the  equations  of  motion: 

d/dt ( 3KE/38 )  -  3KE/3  6  +  3PE/39  =  F  (A.l) 

d/dt(3KE/3U)  -  3KE/3U  +  3PE/3U  =  0  .  (A. 2) 

KE  -  kinetic  energy 

PE  -  potential  energy 

0  -  large  motion  joint  variable,  theta 

U  -  2x1  vector  of  small  motion  displacement  and  slope,  v 
and  <J> 

F  -  generalized,  force  for  large  motion,  applied  moment 

The  actual  motion  of  the  experimental  arm  is  restricted 
to  lie  in  a  vertical  plane.  The  hydraulic  actuator  is 
attached  to  the  base  of  the  arm.  The  load  is  attached  to  the 
end  of  the  arm.  The  large  motion  joint  variable  theta  is  the 
angle  measured  between  the  ERLS  link  and  the  global  coordi¬ 
nate  system  horizontal  axis. 

The  origin  of  the  global  coordinate  system  is  the  axis 
of  the  hydraulic  actuator,  the  base  joint. 

The  horizontal  and  vertical  axes  of  the  global  coordinate 
system  are  parallel  and  perpendicular  to  the  earth.  The  ERLS 
link  is  parallel  to  the  tangent  of  the  experimental  arm  at  the 


]33ss  joint.  Figure  A.l  shows  the  relationships  between  the 
ERLS  and  the  theoretical  arm  position.  The  vector  of  small 
motion  is  limited  to  include  only  the  transverse  displacement, 
v,  and  the  slope,  of  the  end  of  the  arm.  Axial  deformation 
and  torsion  are  neglected  in  the  model  and  are  considered 
insignificant  in  this  application.  The  design  of  the  experi¬ 
mental  arm  to  include  two  parallel  flat  bars  jointed  by  a 
series  of  transverse  bridges  makes  the  arm  rigid  in  torsion. 
Figure  A.l  shows  the  two  components  of  the  vector  of  small 
motion. 


Figure  A.l 

-  -  -  ERLS 

_  Theoretical  Arm  Position 

X , Y  Global  Coordinate  System 

x,y  Local  Coordinate  System 

v  Arm  Tip  Deflection 

■j)  Arm  Tip  Slope 


The  kinetic  energy  of  the  system  includes  contributions 
from  the  arm,  the  loading  and  the  hydraulic  actuator  rotor. 
The  expressions  utilized  for  the  determination  of  the  kinetic 
energy  of  the  system  are  as  follows: 


KE-arm  =  1/2 


U  R  (R)  dv 


ARM 

VOLUME 


(A. 3) 


KE-load  =  1/2  Tr 


(R1)  dv 


LOAD  VOLUME 


(A. 4) 


KE-rotor  =  1/2  Tr 


(Rr)dv 


ROTOR 

VOLUME 


(A. 5) 


Tr  is  the  trace  operation. 

The  global  position  vector  of  the  arm  is  determined  from 
the  following  transformation: 


R  =  W  (r  +  D)  (A. 6) 

W  -  the  3x3  transformation  matrix  and  is  solely  a 
function  of  theta 

r  -  the  3x1  local  position  vector  of  the  arm  measured 
from  the  coordinate  system  whose  origin  is  at  the  end  of 
the  ERLS  link.  Figure  A.l  shows  the  positive  directions 
for  the  local  coordinate  system. 
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D  -  the  3x1  deformation  vector  that  only  includes  the 
transverse  displacement,  v.  In  order  to  introduce  the  nodal 
displacements  at  the  arm  tip  as  the  sole  deformation  variables 
substitution  of  the  shape  function  matrix  and  a  nodal  dis¬ 
placement  vector  is  made  for  D.  The  derivation  of  this 
substitution  is  shown  in  Appendix  C. 

U  -  the  mass  density  of  the  arm,  steel 

The  global  position  vector  of  the  load  is  determined  from 
the  following  transformation: 

R±  =  W  D1  rx  (A. 7) 

-  the  3x3  transformation  matrix  due  to  the  local 
•  deformations  of  the  arm  tip 

r^  -  the  3x1  local  position  vector  for  the  load 

-  the  mass  density  of  the  load,  §teel 

The  global  position  vector  of  the  hydraulic  actuator 
rotor  is  determined  from  the  following  transformation: 

R  =  A  r  (A. 8) 

r  r  r 

Ar  -  the  3x3  transformation  matrix  due  to  the  large 
motion  rotation  of  the  rotor 

rr  -  the  3x1  local  position  vector  for  the  rotor 

y  -  the  mass  density  of  the  actuator  rotor,  aluminum 

The  following  definitions  for  the  inertia  terms  are 
utilized  to  simplify  the  computations  and  the  resultant 
expressions  in  the  equations  of  motion: 


1^  =  f  dv  -  the  3x3  inertia  matrix  of  the  load 


LOAD 

VOLUME 


(  A .  9  ) 


I  =  p  r  r  dv 
r  Hr  r  r 


the  3x3  inertia  matrix  of  the 
actuator  rotor  (A. 10) 


ROTOR 

VOLUME 


1122  (WT,  Wr)  =  f  u  ^>1T  WT  Wr  4>x  dv 


LINK 

VOLUME 


1122  (WT,W)  =  f  m  <t>±T  WT  W  $1  dv 


LINK 

VOLUME 


(A. 11) 


(A. 12) 


1121  (WT,  WQ )  =  f  u  ^>1  WT  WQ  r  dv 


LINK 

VOLUME 


(A. 13) 


Illl(WgT,  Wg)  =  f  U  rT  WgT  Wg  r  dv 


LINK 

VOLUME 


1112  (W0T ,  W)  =  f  u  rT  W0T  w  <P1  dv 


LINK 

VOLUME 


I122(W„  ,  W) 


\1  WpT  W  <J> i  dv 


LINK 

VOLUME 


(A. 14) 


(A. 15) 


(A. 16) 


mmmmm 


LINK 

VOLUME 


(A. 17) 


I122(W5T,  W0) 


W, 


*1 


dv 


(A. 18) 

(A. 19) 

Computation  of  the  preceding  link  inertia  matrices  require 
utilization,  of  the  3x2  link  shape  matrix  the  3x1  link 

local  position  vector  r,  and  various  combinations  of  the  3x3 
transformation  matrix  W.  The  expression  WQ  implies  a  deriva¬ 
tive  with  respect  to  the  large  motion  joint  variable,  theta. 

The  expression  W  results  from  a  simplification  of  the  second 
time  derivative  of  the  transformation  matrix  W  and  is  termed  a 
residual  acceleration.  Further  details  on  the  derivation  of 
these  expressions  can  be  found  in  Reference  2  and  a  listing  of 
these  matrices  is  found  in  Appendix  B  in  the  computer  coding. 

The  potential  energy  of  the  system  includes  contributions 
from  the  strain  energy  of  the  arm  due  to  deformation  and  from 
the  gravitational  energy  of  the  load  and  the  arm.  The 
expressions  utilized  for  the  determination  of  the  potential 
energy  of  the  system  are  as  follows: 


‘yy 


K1 
LOAD 
VOLUME 


2  , 
y  dv 


xx 


p  ^  x  dv 


LOAD 

VOLUME 


PEa 


1/2  j  E  Izz  (V 

LINK 

LENGTH 


dx  -  potential  energy  due  to 
deformation  (A. 20) 
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1  T 

PE  =  -  u  r  g  dv  -  potential  energy  of  the  link 

~  due  to  gravitation  (A.  21) 

LINK 
VOLUME 


T 

PE,  =  ~  j  ri  9  dv  -  potential  energy  of  the  load 

^  due  to  gravitation  (A. 22) 

LOAD 
VOLUME 


E  I  -  the  flexural  rigidity  in  the  z  direction,  or 
perpendicular  to  the  plane  of  motion. 

v' '  -  the  second  derivative  of  the  transverse 

displacement  v  with  respect  to  the  x  local  coordinate 
direction.  In  order  to  introduce  the  nodal  displacements 
at  the  arm  tip  as  deformation  variables  substitution  of  the 
second  derivative  of  the  shape  function  matrix  with  respect 
to  the  x  coordinate  direction  and  a  nodal  displacement 
vector  is  made  for  v''.  The  derivation  of  this  substitution 
is  shown  in  Appendix  C. 

g  -  the  gravitational  acceleration  vector 


The  following  definitions  are  utilized  to  simplify  the 
computations  and  the  resultant  expressions  in  the  equations 
of  motion: 


K . .  =  J  fT  C  T  dx  -  the  2x2  stiffness  matrix  (A. 23) 

LINK 

LENGTH 


T 

H.  .  =  j  . .  r  dv  -  the  link  first  moment  of  inertia 

J  vec tor  ( A . 2 4 ) 

LINK 
VOLUME 
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dv  -  the  link  shape  matrix  first 
moment  of  inertia  vector 


(A. 25) 


■  A  T 

LINK 

VOLUME 


H 


41 


r1 


dv 


LOAD 

VOLUME 


the  load  first  moment  of  inertia 
vector  (A. 26) 


T  -  the  second  derivative  of  the  shape  function  matrix. 

C  -  the  3x3  flexural  rigidity  matrix  including  only 

E  I  . 
zz 

Substitution  of  the  expressions  for  kinetic  energy  and 
potential  energy  into  the  Lagrange  equations  and  after  much 
computation  and  simplification  results  in  the  following  two 
non-linear,  coupled,  second-order,  ordinary,  differential 
equations  for  the  large  and  small  motions  of  the  single-link 
flexible  arm: 


MQQ  9  +  MQN  U  =  FQ  (A.  27) 

MNQ  0  +  MNN  U  +  KN  U  =  FN  (A. 28) 

The  following  are  definitions  for  the  coefficients: 

MQQ  =  I111(W9T,  Wg)  +  UT  1122  (WgT,  W0)  U  +  Trace(W0 

D  T  W  T  +  A  I  A  T)  (A. 29) 

1  9  r  r  r 

MQN  =  1112 (W.T,  W)  +  ( (M,  L  +  M  ) ,  (M  L  +  I  +  I  )) 
v  ’j  '  I  x  x  xx  yy 

(A. 30) 
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L  is  the  link  length.  is  the  first  moment  of  the  load 

with  respect  to  the  local  coordinate  y  axis.  is  the  mass 

of  the  load. 

FQ  =  2.  UT  I122(W0T,  W)  U  +  W0T  g  +  UT  H21  W0T  g  - 

Trace  (WQD.  I.  D.T  W  +  2.  W-D.  I.  D.T  W  +  A  a  I  A  )  + 
8111  r  6111  r0r  rr 

H41  °1T  wqT  9  +  T  (A. 31) 


T  is  the  externally  applied  torque.  A  results  from  a 
simplification  of  the  second  time  derivative  of  the  transfor¬ 
mation  matrix  A  and  is  termed  a  residual  acceleration.  U 

r 

is  the  2x1  nodal  displacement  vector  containing  the  link  tip 
deflection,  v(0)  ,  and  slope,  <j>(0).  The  expression  A^q 
implies  a  derivative  with  respect  to  the  large  motion  joint 
variable,  theta. 


MNQ  =  (Trace  (W^  I±  Di;lT  WT)  ,  Trace  (W0  D1  I1  D12T  WT)  ) 
+  I 1 2 1 ( WT ,  W_)  (A. 32) 

D11  an<^  °12  are  t^ie  ^erivatives  °f  the  arm  tip 
deformation  transformation  matrix  with  respect  to  each  nodal 
displacement,  deflection  and  slope,  respectively. 


MNN  =  1122 (W,W)  + 


”l 


v  (0) 


(A. 33) 


0 


I 

xx 


+ 


(0) 
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v(0)  and  MO)  are  arm  tip  deflection  and  slope 


accelerations  respectively. 

KN  =  K11  +  i122(wT'  W  )  (A. 34) 

FN  =  H21  WT  g  -  (Trace(Wr  I1  D1;lT  WT  +  2 .  W  D1  I1  Di;lT 
WT)  ,  Trace  (W^.  D1  I1  D12T  WT  +  2 .  W  D1  I1  D12T  WT)  ) 

+  (H41  Dx1T  WT  g,  H41  D,/  WT  g)  (A.  35) 


The  numerical  values  utilized  for  the  experimental 
flexible  arm  system  variables  are  listed  in  Appendix  B  and 
are  in  SI  units. 


*  *  * 


APPENDIX  B 


LISTING  OF  THE  FORTRAN  CODING  UTILIZED  IN  SOLVING 
THE  DYNAMIC  EQUATIONS  OF  MOTION  FOR  THE  EXPERIMENTAL 
SINGLE-LINK  FLEXIBLE  ARM 


*  SIMULATION  OF  SINGLE  LINK  FLEXIBLE  MANIPULATOR  DYNAMICS 

x  - 

x 

X 

x  THIS  PROGRAM  SOLVES  THE  ERLS  FLEXIBLE  MANIPULATOR  DYNAMICS  FOR  A 
X  SINGLE  LINK  EXPERIMENTAL  ARM.  THE  EXPERIMENTAL  ARM  PARAMETERS  ARE 

X  INPUTTED  AND  THE  HYDRAULIC  ACTUATION  DYNAMICS  ARE  INCLUDED  IN  THE 

X  SIMULATION.  THE  INPUT  IS  THE  CURRENT  TO  THE  SERVOVALVE  MOUNTED  ON 

X  THE  HYDRAULIC  ACTUATOR  AND  THE  OUTPUT  IS  THE  POSITION  OF  THE  ARM 

X  TIP  IN  THE  GLOBAL  REFERENCE  SYSTEM.  THE  CODING  CONSISTS  OF  A  MAIN 

X  PROGRAM  AND  FIFTEEN  SUBROUTINES  AND  ARE  DESCRIBED  BELOW, 
x 

x  THE  FOLLOWING  PARAMETERS  ARE  DEFINED: 
x  1 . A-EFFECTIVE  CROSS-SECTIONAL  AREA  OF  FLEXIBLE  ARM 

x  2 . ARRDD-3X3  SECOND  TIME  DERIVATIVE  OF  ROTOR  RESIDUAL  ACCELERATION 

x  MATRIX 

x  3 .  ARTH-3X3  ROTOR  TRANSFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT 
X  TO  THETA 

X  4 . BE-EFFECTIVE  BULK  MODULUS  OF  FLUID 

x  5.BIGF-3X1  RIGHT-HAND  SIDE  VECTOR  FOR  LARGE  AND  SMALL  MOTION 
x  ACCELERATIONS 

X  6 . BIGM-3X3  MATRIX  OF  LARGE  AND  SMALL  MOTION  ACCELERATION 
X  COEFFICIENTS 

x  7 . CTM-TOTAL  LEAKAGE  COEFFICIENT  OF  THE  ACTUATOR 
X  8 . DEFM-DISPLACEMENT  DEFORMATION  VARIABLE 

X  9 . DEFMD-TIME  DERIVATIVE  OF  DISPLACEMENT  DEFORMATION  VARIABLE 

x  10. DIFF,QERR,QERR1, FACTOR-DUMMY  VARIABLES 

X  11 . DL1-3X3  DEFORMATION  MATRIX 

x  12 . DL11-3X3  DEFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO  THE 
X  DISPLACEMENT  DEFORMATION  VARIABLE 

x  13 . DL12-3X3  DEFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO  THE 
X  SLOPE  DEFORMATION  VARIABLE 

x  14 . DL1D-3X3  FIRST  TIME  DERIVATIVE  OF  DEFORMATION  MATRIX 
X  15 . DM-ACTUATOR  DISPLACEMENT 

x  16. E-MODULUS  OF  ELASTICITY  OF  STEEL 

X  17.FN-2X1  RIGHT-HAND  SIDE  VECTOR  FOR  SMALL  MOTION  ACCELERATIONS 

X  18 . FQ-RIGHT-HAND  SIDE  FOR  LARGE  MOTION  ACCELERATIONS 
X  19.G-3X1  GRAVITATIONAL  ACCELERATION  VECTOR 

X  20 . GPOS-3X1  GLOBAL  POSITION  VECTOR  FOR  ARM  TIP 

X  21.H11-1X3  LINK  FIRST  MOMENT  OF  INERTIA  VECTOR 

X  22 . H21-2X3  LINK  SHAPE  MATRIX  FIRST  MOMENT  OF  I NERTI AVECTOR 

X  23 . H41-1X3  LOAD  FIRST  MOMENT  OF  INERTIA  VECTOR 

x  24 . KCE-TQTAL  FLOW  PRESSURE  COEFFICIENT 

X  25 . PL-LOAD  HYDRAULIC  PRESSURE  DROP 

X  26 .PS-HYDRAULIC  SUPPLY  PRESSURE 

x  27 . QL-FLOW  DELIVERED  FROM  THE  SERVOVALVE 

X  28. SLOP-SLOPE  DEFORMATION  VARIABLE 

x  29 . SLOPD-TIME  DERIVATIVE  OF  SLOPE  DEFORMATION  VARIABLE 
X  30 . SOL -3X1  VECTOR  OF  LARGE  AND  SMALL  MOTION  ACCELERATIONS 

X  31 . TE-TORQUE  EFFICIENCY 

X  32.TH-LARGE  MOTION  POSITION  VARIABLE 

X  33 . THD-TIME  DERIVATIVE  OF  LARGE  MOTION  VARIABLE 
X  34 . TORQUE-APPLIED  TORQUE  BY  ACTUATOR 

x  35.U-2X1  ARM  TIP  DEFORMATION  VECTOR  INCLUDING  DISPLACEMENT  AND  SLOPE 
x  36.UD-2X1  ARM  TIP  DEFORMATION  VECTOR  DIFFERENTIATED  WITH  RESPECT  TO 
x  TIME 

x  37 . VT-TOTAL  COMPRESSED  VOLUME  INCLUDING  ACTUATOR  LINES  AND  CHAMBERS 
X  33.W-3X3  LINK  TRANSFORMATION  MATRIX 

x  39.WD-3X3  FIRST  TIME  DERIVATIVE  OF  LINK  TRANSFORMATION  MATRIX 

x  40 . WRDD-3X3  SECOND  TIME  DERIVATIVE  OF  LINK  RESIDUAL  ACCELERATION 
MATRIX 

41  .  WTH-3X3  TRANSFORMATION  MATRIX  DIFFERENTIATED  WITH  RESPECT  TO 
THETA 
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*  *  *  *  *  * 


*  VALVE 

*  42. XI IMP -CURRENT  INPUT  EQUAL  TO  INITIAL  AND  FRACTIONAL  AMOUNTS 

*  43.XIL-3X3  INERTIA  MATRIX  OF  THE  LOAD 

*  44 . XIO-INITIAL  INPUT  CURRENT  TO  SERVOVALVE 

*  45 . XIR-3X3  ROTOR  INERTIA  MATRIX 

x  46 .XISTEP-STEP  INPUT  OF  FRACTIONAL  AMOUNT  OF  INPUT  CURRENT 

x  47.XK11-2X2  PARTIAL  LINK  STIFFNESS  MATRIX 

X  48.XKN-2X2  LINK  STIFFNESS  MATRIX 

x  49 . XKV-SERVOVAL VE  SIZING  CONSTANT 

x  50 . XLL-LENGTH  OF  FLEXIBLE  ARM 

x  51. XML -MASS  OF  LOAD 

X  52 . XMNN-2X2  COEFFICIENT  MATRIX  OF  SMALL  MOTION  ACCELERATIONS  IN  THE 
X  SMALL  MOTION  DYNAMIC  EQUATIONS 

X  53 . XMNQ-2X1  COEFFICIENT  VECTOR  OF  LARGE  MOTION  ACCELERATIONS  IN  THE 
x  SMALL  MOTION  DYNAMIC  EQUATIONS 

x  54 . XMQN-1X2  COEFFICIENT  VECTOR  OF  SMALL  MOTION  ACCELERATIONS  IN  THE 
X  LARGE  MOTION  DYNAMICS  EQUATION 

X  55 . XMQQ-COEFFICI ENT  OF  LARGE  MOTION  ACCELERATION  IN  THE  LARGE  MOTION 
X  DYNAMICS  EQUATION 

X  56 . XMQQP-2X2  DUMMY  MATRIX  FOR  USE  IN  FORMULATING  THE  EQUATIONS  OF 
X  MOTION 

X  57 . XMR-MASS  OF  ACTUATOR  ROTOR 
X  '  58 . XMU-MAS5  DENSITY  OF  STEEL  FLEXIBLE  ARM 

X  59 . XMX-FIRST  MOMENT  OF  LOAD  WITH  RESPECT  TO  THE  LOCAL  COORDINATE 
X  Y  AXIS 

x  60 .XXI -VARIABLE  REPRESENTING  INERTIA-LIKE  LOAD  PROPERTY 

x  61 . YYI-VARIABLE  REPRESENTING  INERTIA-LIKE  LOAD  PROPERTY 

x  62 . ZI-AREA  MOMENT  OF  INERTIA  OF  FLEXIBLE  ARM 

x 

x 

x 

x 

INITIAL 

x 

X  INITIAL  VALUES  OF  PARAMETERS  ARE  INPUTTED  VIA  XINIT  SUBROUTINE 

x 

x 

/  REALX8  U(2.1),XMQQ(n,XMQQP(2.2),DLlC3,3),WTH(3,3),ARTH(3,3), 

/  #XIR(3,3),XMQN(1,2),UDC2,1),H11U,3),G(3,1),H21(2,3), 

/  tURDD(3,3>,DLlD(3,3).WD(3,3),ARRDD(3,3),H41Cl,3),XKll(2,2), 

/  #DL12(3.3),XMNQC2,1),W(3,3),XMNN<2,2),XKN(2,2),FN<2,1),BIGM<3,3), 

/  •BIGF(3.1>,XIL<3,3),DL11C3,3),DEFMDC1),S0LC3),THDU),SL0P(1), 

/  #SL0PDC1),A<1),E(1),ZI<1),XXI(1).YYI(1),F9C1),GP0SC3),XITH(1), 

/  #XMU( 1 ) , XLL ( 1 ) , XML ( 1 ) , XMR(1 ) , XMXC 1 ) , THU ) , TORQUEC 1 ) , DEFMC 1 ) , 

/  tPS( 1 )  >  XIFRACt 1 ) , XIO( 1 ) . KCEC 1),VT(1),BE(1),  DM( 1 ) , XKV( 1 ) , TEC  1 ) > 

/  #<JL(1).PL(1)»DIFF(1)#XIINP(1)» QERR1 (1 ) >  QERRC 1 ) , FACTORC 1), XI STEP (1) 

FIXED  I 
NOSORT 

SYSTEM  DPINTG 

4TH  ORDER  RUNGE-KUTTA  DOUBLE-PRECISION  INTEGRATION 
METHOD  RKSDP 


INITIALIZATION  SUBROUTINE 


CALL  XINITCTH,THD,DEFM,DEFMD,SLOP,SLOPD,VO,POSO.A,XML,XMU, . . . 
XLL,XMR,E,ZI.PS,XIFRAC,XIO,CIM,VT, BE,  DM, XKV, TE, QL , . . . 

PL , PL IC ) 


DYNAMIC 


COEFFICIENTS  FOR  BOTH  LARGE  AND  SMALL  MOTION  ACCELERATIONS 
AND  THE  RIGHT-HAND  SIDES  ARE  COMPUTED  IN  THE  FOLLOWING 
SUBROUTINES.  ALSO, THE  HYDRAULIC  DYNAMICS  ARE  INCLUDED 
IN  THE  MAIN  PROGRAM. 


NOSORT 
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t 

if 


*  HYDRAULIC  DYNAMICS 

X 

XISTEPC 1 ) =XIFRAC( l)xSTEP(O.Q) 

XIINP( 1 ) =XIO( 1 )+XISTEP( 1 ) 

I  F(  PL ( 1 ) .GT.PS(l))  GO  TO  2 
GO  TO  3 

2  PL(1)=PS(1) 

3  QERR1(1)  =  (XIINP(1)XXKV<1)XDSQRTCPS(1)-PLU) )  )-(DM(  1  )*THD(1 ) ) 

QERRC 1 ) =QERR1 (1)/KCE(1) 

DIFF( 1 )=QERR( 1 )-PL ( 1 ) 

FACTOR ( 1)=VT(1)/(4,0D0XBE(1)XKCE(1)) 

DIFF1 ( 1 ) =DIFF< I ) 

SORT 

PL1=INTGRL(PLIC,DIFF1,1) 

NOSORT 

PL(1)=PL1(1 )/ FACTOR! 1 ) 

TORQUEC 1)=TE(1)XPL(1)XDM(1) 
x 

X  MATRIX  AND  VECTOR  FORMULATION  SUBROUTINE 

x 

CALL  F0RM(W,WTH,WD,DL1,DL1D,XIL,XIR, ARTH, WRDD, ARRDD, U , UD, . . . 
XMQQP,G.H11,H21,DL11,DL12,H41,XK11,A,XMU,XML,XLL,TH,THD, . . . 

DEFM, DEFMD, SLOP, SLOPD,E,ZI,XMR, XMX, YYI, XXI) 

x 

X  COEFFICIENT  OF  LARGE  MOTION  ACCELERATION  IN  LARGE  MOTION  DYNAMICS 
X  EQUATION  SUBROUTINE 

x 

CALL  XLMMQQCXMQQ, U, XMQQP, DL1 , WTH, ARTH, XIL ,XIR, A, XMU, TH, DEFM, SLOP) 

x 

X  COEFFICIENTS  OF  SMALL  MOTION  ACCELERATIONS  IN  LARGE  MOTION  DYNAMICS 

X  EQUATION  SUBROUTINE 

x 

CALL  XLMMQNL XMQN , A , XMU , XML , XL L , XMX , SL OP , DEFM , YYI , XXI ) 
x 

x  RIGHT-HAND  SIDE  FOR  LARGE  MOTION  DYNAMICS  EQUATION  SUBROUTINE 
x 

CALL  XLMFQ(FQ,U, XMQQP, DU, WTH, ARTH. XIL, XIR, UD, Hll , G, H21 , WRDD,  .  .  . 

Dl 1 D, WD. ARRDD, H41 , TH, THD, DEFM, DEFMD, SLOP, SLOP D, A , XMU , XML , XLL , .  .  . 
lr  RQUE) 
x 

x  LINK  STIFFNESS  MATRIX  SUBROUTINE 

x 

CALL  SMKN< XKN , XK1 1 , XMQQP , A , XMU , THD ) 
x  • 

x  COEFFICIENTS  OF  LARGE  MOTION  ACCELERATION  IN  SMALL  MOTION 
x  DYNAMICS  EQUATIONS  SUBROUTINE 

X 

CALL  SMMNQCXMNQ , DL1 , WTH, XI L  ,  DL  1 1 »  DL  1 2,  W,  TH , DEFM, SLOP, A, XMU ) 
x 

X  RIGHT-HAND  SIDE  OF  SMALL  MOTION  DYNAMICS  EQUATIONS  SUBROUTINE 

X 

CALL  SMFN(FN,H21,W,G, WRDD, DL1, XIL, DL 11 , DL 12, WD, DL1D, H41 , TH, . . . 

THD, DEFM, DEFMD, SLOP, SLOPD) 
x 

X  COEFFICIENTS  OF  SMALL  MOTION  ACCELERATIONS  IN  SMALL  MOTION  DYNAMICS 
x  EQUATIONS  SUBROUTINE 

X 

CALL  SMMNN (XMNN, XMQQP , XML , A , XMU , XX I , YY I , XMX ) 
x 

x  ACCELERATION  COEFFICIENTS  MATRIX  AND  RIGHT-HAND  SIDE  VECTOR 

x  FORMULATION  SUBROUTINE 

x 

CALL  BIGFOR ( BIGM, BIGF, XMQQ, XMQN, FQ.XMNQ, XMNN, XKN, FN,U) 
x 

x  LINEAR  EQUATION  SOLVER  FOR  ACCELERATIONS  SUBROUTINE 

x 

CALL  XLEQ(BIGM, BIGF, SOL) 
x 

x  TRANSFORMATION  FROM  LOCAL  COORDINATE  TO  GLOBAL  COORDINATE  TIP 

X  POSITION  SUBROUTINE 

x 

CALL  GLOB(GPOS.W, DEFM) 
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*  *  *  *  w  *  x  iuhououu 


CALL  GLOB(GPOS,W,DEFM) 

INTEGRATE  ACCELERATIONS  AND  THEN  VELOCITY  TO  GET  LARGE 
ANGULAR  POSITION  AND  SMALL  MOTION, LOCAL  COORDINATE, TIP 
DO  5  1*1,3 
SOLI ( I ) =SOL ( I ) 

5  CONTINUE 
SORT 

VEL*INTGRL(V0.S0L1,3) 

NOSORT 

THD( 1 ) =VEL ( 1 ) 

DEFMDC 1 ) =V£L (2) 

SLOPDC 1 )*VEL(3) 

DO  10  1=1,3 
VEL 1(1) =VEL ( I ) 

10  CONTINUE 
SORT 

POS=INTGRL (POSO, VEL1 , 3 ) 

NOSORT 

THC 1 ) =POS( 1 ) 

DEFM(l)=POS(2) 

SL0P(1)=P0S(3) 

AC1=ZZRND( SOL  1 ( 1 ) ) 

AC2  =  ZZRND( SOL  1 ( 2) ) 

AC3=ZZRND( SOLI ( 3 ) ) 

VE1=ZZRND( VEL ( 1 )  ) 

VE2=ZZRND(VEL(2)  ) 

VE3=ZZRND(VEL( 3) ) 

P01=ZZRND(P0S( 1 )  ) 

P02=ZZRND(P0S(2) ) 

P03=ZZRND(P0S( 3)  ) 

XPOS=ZZRND(GPOS( 2)  ) 

YPOS=ZZRND(GPOS(3) ) 

TORK=ZZRND(TORQUEC 1 ) ) 

LPD=ZZRND(PL( 1 ) ) 

OUTPUT  GLOBAL  COORDINATE  TIP  POSITION 


TERMINAL 
OUTPUT  XPOS 
OUTPUT  YPOS 
OUTPUT  TIME, XPOS 
PAGE  XYPLOT 

LABEL  X-POSITION  <L0AD=.4233  KG  ,I=1.02MA) 

OUTPUT  TIME, YPOS 
PAGE  XYPLOT 

LABEL  Y-POSITION  (LOAD*. 4233  KG  ,I=1.02MA> 

TIMER  FINTIM  =1.5  .  OUTDEL  =  0.01  ,  DELMIN  =  5.0E-8 
'ID 

rop 


LISTING  OF  SUBROUTINES 


SUBROUTINE  XINITCTH , THD, DEFM, DEFMD, SLOP, SLOPD, VO, POSO, A 
#MR, E, ZI , PS, IFRAC, IO.CTM, VT, BE, DM, KV, TE, QL , PL , PL IC) 
REALX8  V0(3),P0S0(3),ML,MU,LL,MR,TH,THD,DEFM, DEFMD, SLOP 
#A,E,ZI,ITORQ, PS, IFRAC, 10 , CTM, VT, BE, DM, KV, TE, QL , PL , PLIC, 
ITORQ=  27 . 39303003D+Q0 
DM=6 . 227  ID-05 
TE= . 9D+00 
PL  =  ITORQ/( DM*TE) 

PLIC=PL 

CTM=3 .7064772D-13 

QL=CTM*PL 

KV=2 . 402963D-09 

PS=1 .37883D+07 

IO  =  QL/(KV*DSQRT(PS-PU) 

IMAX=10 . D+00 

IFRAC* . 5D+QQX< IMAX-IO) 


MOTION 

POSITION 


, ML , MU, LL , 

, SLOPD, 
IMAX, 


on  o 


BE=690 . D6 
A=6 . 17795D-04 
ML*. 42330000000000 
MU=7S61. 05000000000000 
L L  =  0  .  99850000000000 
MR=9 . 00011451000000 
E=2 . 0D1 1 
ZI=4 . 065D-10 
V0(1)=0 .0  00000000000000 
V0( 2 1=0.000000000 000 000 
V0< 3 >=0.000000000000000 
P0S0(1) =0.00000000000000 
P0S0( 2 >=-.07804116400000 
P0S0< 3 >=-.09875534300000 
TH=POSO( 1 ) 

THD=V0( 1 > 

DEFM=P0S0(2) 

DEFMD=V0(2) 

SL0P=P0S0( 3) 

SL0PD=V0( 3) 

RETURN 

END 


SUBROUTINE  F0RM< W, WTH. WD, DL1 , DL1 D, XI L , XIR, ARTH, WRDD, ARRDD, U, UD, 
*XMQQP,G,Hll,H21,DLll,DL12,H41,Xiai,A,MU,ML,LL,TH,THD,DEFM,DEFMD, 
iSLOP, SLOPD, E, ZI , MR.MX, YYI , XXI ) 

REALMS  H(3,3>,WTH(3,3),WD(3,3),DL1(3,3>,DL1DC3,3),XIL(3,3) 

REALX8  XIRC3,3>,ARTH(3,3),WRDD(3,3> , ARRDDC 3, 3) , UC 2, 1 > , UD( 2, 1 > 

REAL*8  XMQQP(2,2>,G(3,l>,Hlia,3>,H21(2,3),DLllC3,3),DL12(3,3) 

REAL*8  H41d.3>,XKll(2,2> , MU , ML , LL , MR , MX , TH 

REAL*8  THD,DEFM,DEFMD,SLOP,SLOPD,XXI,YYI,A,E,ZI 

W(l, 1>=1. 00000000000000 

MCI, 2 >=0.00000000000000 

MCI, 3)=0. 00000000000000 

W(2,1)=LL*DC0S(TH> 

H(2.2)=DC0SCTH) 

W(2,3>=-DSIN(TH) 

WC3.1)=LL*DSIN(TH) 

WC3,2)=DSIN(TH> 

WC3,3)=DC0S(TH> 

WTH( 1,1 >=0.000000 00000000 
WTH( 1, 2 )=0. 00000000800000 
WTH( 1 , 3 ) =0 .00000000000000 
WTH(2,1)=-LL*DSINCTH> 

WTH<  2, 2>  =-DSINCTH) 

WTH( 2, 3 ) =-DCOS( TH  > 

WTH(3.1>=LL*DC0S(TH) 

WTH( 3 , 2)  =  DCOS( TH ) 

WTH(3, 3>=-DSIN(TH> 

WD(1,1>=0. 00000000000000 
WD< 1,2 >=0.0000000000 00 00 
WD( 1,3 >=0.0000000000 0000 
WD(2,1)=-LL*DSIN(TH)XTHD 
WD(2,2)=-DSIN(TH>*THD 
MD( 2, 3 ) =-DCOS( TH ) *THD 
HD(3,1)=LL*DC0S(TH)*THD 
WD(3,2)=DCQS(TH)*THD 
WD(3,3)=-DSIN(TH)*THD 
DL1( 1,1 >=1.00000000000000 
DL  1(1 , 2) =0.00000000000000 
DL1(1,3> =0.00000000000000 
DL  1(2,1 >=0.000000 00000000 
DL  1(2, 2>  =  1. 00 000000 000000 
DL 1  (  2, 3 ) =-SL0P 
DL 1  (  3 , 1 ) =DEFM 
DL 1 ( 3 , 2>  =SL0P 
DL 1  (  3 , 3 >  =  1 .00000000000000 
DLlDd,  1 )  =0 . 00000000000000 
DL1 DC  1,2 >  =  0.00000000000000 
DLlDd ,3) =0 . 00000000000000 
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DL 10(2.1) =0.00000000000000 

DL1DC 2, 2) =0.0000000000000 

DL1DC2, 3)=-SL0PD 

DL1D(3. 1)=DEFMD 

DL1DC3,2)=SL0PD 

DL 1D( 3, 3)=0. 00000000000000 

XILC 1 , 1 ) =ML 

XI l (1,2) =0.00067 1418 00000 
XIL ( 1 >  3) = . 000000000  00000 
XIU2,1)  =0.00067 141800000 
XIL(2,2)=1 .4220-06 
XIL (2, 3 >=0.00000000000000 
XIL (3,1 >=.000000 00000000 
XIL (3, 2 >=0.00000000000000 
XIL(3,3)=5.97754D-04 
MX=. 00067141800000 
XXI=1.422D-06 
YYI=5.97754D-04 
XIR( 1, 1 ) =MR 

XIRC1, 2 >=0.000000000000000 
XIR( 1, 3 >=0. 000000000000000 
XIR( 2,1 >=0.000000000000000 
XIR( 2, 2) =.027467 130000000 
XIR( 2, 3) =0.0000000000000 00 
XIR( 3,1 >=0.00000000 0000000 
XIR( 3, 2) =0.000000000000000 
XIR(3, 3) =.027 467 13000000 
ARTH< 1,1 >=0.00000000000000 
ARTH( 1,2) =0.00000000000000 
ARTHC 1,3 >=0.00000000000000 
ARTH< 2,1 >=0.00000000000000 
ARTH(2,2)=-DSINCTH) 
ARTH(2,3>=-DC0S(TH> 

ARTH( 3,1 >=0.00000000000000 
ARTH(3,2)=DC0SCTH) 

ARTHC  3, 3)=-DSINCTH) 

WRDDC 1,1 >=0.00000000000000 
WRDD< 1,2) =0.0000000000 0000 
WRDDC 1,3) =0.00000000000000 
HRDD(2, 1 ) =-LL*DC0S(TH)x(THDX*2) 
HRDD(2, 2)=-DC0S(TH)*(THDX*2) 
WRDDC2,3)=DSIN(TH)*CTHD**2) 
WRDD(3,1)=-LL*DSINCTH)X(THDX*2) 
WRDDC3.2)=-DSINCT.H)*CTHDX*2) 
WRDDC  3, 3) =-DCOSC  TH )#CTHDX#2 ) 
ARRDD(1,1)=0. 00000000000000 
ARRDDC 1, 2) =0. 00000000000000 
ARRDO( 1, 3 )=0. 00000000000000 
ARRDD( 2, 1)=0 .00000000000000 
ARRDDC2,2)=-DC0SCTH)XCTHD**2) 
ARRDDC2»3)=DSINCTH)XCTHDXX2) 
ARRDDC 3. 1)=0. 00000000000000 
ARRDDC  3,2)=-DSIN(TH)X(THD**2) 
ARRDDC 3, 3)=-DC0SCTH)xCTHDXX2) 

UC 1 , 1 )=DEFM 
UC  2, 1 ) =SL0P 
UDC 1 , 1 ) =DEFMD 
UDC2, 1 )=SLOPD 

XMQQPC 1,1 >=.3714286 0000 000 
XMQQPC 1,2)=-. 05238100000000 
XMQQPC2,1)=-. 05238100000000 
XMQQPC 2, 2)=. 00952380000000 
GC1,1)=0. 00000000000000 
GC2,1 >=0.00000000000000 
GC 3.1 >=-9.8066 0000000000 
HI 1C  1,  1  ) =4. 8 5651900000000 
HI  1C  1,2 >=-2.4282586 9 300 0  00 
HI 1C  1,3)=0. 00000000000000 
H21C1, 11=0.00000000000000 
H21C 1,21=0.00000000000000 
H21C1,3)=A*MU*C .50000000000000) 
H21C 2,1 >=0.0000000 0000000 
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H21( 2, 2)  =0.00000000000000 
H21 (2, 3 )=A*MU*(-. 08333333333333) 
DO  50  1=1,3 
DO  60  J=1 , 3 

DL1KI,  J)=0. 00000000000000 
DL 12( I, J) =0.00000000000 000 
60  CONTINUE 
50  CONTINUE 

DL 11C 3, 1)=1 .00000000000000 
DL 12(2, 3 >=-1.00000000000000 
DL 12(3, 2  )  =  1 .00000000000  00  0 
H41 ( 1 , 1 >  =ML 

H41(l,2)=0. 000671418000000 
H41 (1,3)=. 00000 0000000000000 
XK1 1(1,1) =12. U0000000000000*E*ZI 
XK1 1(1, 2 )=-6. 000000 000000*E*ZI 
XKll(2,l)=-6 .0000000000 00*E*ZI 
XK11(2,2)=4. 00000 0000 0000*E*ZI 
RETURN 
END 


SUBROUTINE  XLMMQQ(MQQ, U, XMQQP , Dll , WTH, ARTH, XIL , XIR, A, MU, TH, DEFM, 
•SLOP) 

REALX8  MQQ, UT (1,2),P(1,2),DL1T(3,3), WTHT (3,3),ARTHT(3,3),P1(3,3) 

REALX8  P2(3,3),P3(3,3),P4(3,3),P5(3,3),P6(3,3),P7(3,3),MU 

REALX8  U( 2, 1 ) , XMQQP ( 2 ,2),DL1(3,3), WTH( 3,3), ARTH(3, 3),XIL(3,3) 

REALX8  XIR( 3, 3) , A, TH, DEFM, SLOP, SP, TP 

M=2 

L  =  1 

N=3 

MQQ=0. 00000000000000 
CALL  TRANS(U,UT,M,L) 

CALL  MATMUUUT, XMQQP, L,M,M,P) 

CALL  MATMUL (P,U,L,M,L,SP) 

CALL  TRANS(DL1,DL1T,N,N) 

CALL  TRANS ( ARTH, ARTHT , N, N) 

CALL  TRANS (WTH, WTHT ,N,N) 

CALL  MATMUL ( WTH, DL1,N,N,N, PI) 

CALL  MATMUL ( PI , XIL , N, N,N, P2) 

CALL  MATMUL (P2,DL1T,N,N,N,P3) 

CALL  MATMUL ( P3 , WTHT , N , N , N , P4 ) 

CALL  MATMUL (ARTH, XI R,N,N,N,P5) 

CALL  MATMUL ( P5, ARTHT , N , N, N, P6  ) 

CALL  MATADD(P4,P6,N,N,P7) 

CALL  TRACE( P7 , N , TP ) 

MQQ=((1./3.)*A*MU)  +  (AXMUXSP)  +  TP 

RETURN 

END 


SUBROUTINE  XLMMQN(XMQN , A , MU , ML , LL , MX , SLOP , DEFM, YYI , XXI ) 
REALX8  XMQN( 1, 2), MU, ML, LL, MX, A, SLOP, DEFM, YYI, XX I 
XMQN( 1,1)=(AXMUX( . 3500  0  000000  000) )  +  ( ML  XL L)+MX 
XMQN(1, 2 )=(A*MUX(-. 05000000000000 ))+(MXxLL)+YYI+XXI 
RETURN 
END 


SUBROUTINE  XLMFQ(FQ, U, XMQQP, DL1 , WTH, ARTH, XIL , XIR, UD, HI 1 ,  G, H21 , 
•WRDD , DL ID, WD, ARRDD, H41 , TH, THD, DEFM, DEFMD, SLOP , SLOPD , A , MU , ML , LL , 
•TORQUE) 

REAL*8  FQP(2,2),P(I,2),P1(1,3),P2(1,3),P3(1,3),P4(3,3),P5(3,3) 
REALMS  P6(3,3),P7(3,3),P8(3,3),P9(3,3),P10(3,3),P11(1,3),P12(1,3) 
REALX8  FPFH( 3, 3 ) , FHP( 3,3),FPT(3,3),DL1DT(3,3), WDT ( 3 , 3 ) , ARRDDT ( 3 , 3 ) 
REALX8  UT(1,2),DL1T(3,3), WRDDT (3.3),FPF(3,3), FPS( 3,3), WTHT (3,3) 
REAL  x8  U( 2, 1 ) , XMQQP ( 2,2), DL1( 3, 3) , WTH( 3, 3) , ARTH( 3, 3) , XIL( 3, 3) 

REAL *8  XIR(3,3),UD(2,1),H11(I,3),G(3,1),H21(2,3), WRDD( 3,3) 

REAL *8  DL1D(3,3),WD(3,3),ARRDD(3,3),H41(1,3) ,MU,LL,ML 
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non  ooo 


REAL*8  A, TORQUE, FQ.TH. THD. DEFM, DEFMD, SLOP. SLOPD 

REALX8  FP,SP,TP,TFP,FTHP 

M=2 

L  =  1 

N=3 

CALL  TRANS ( U . UT ,M, L  ) 

DO  10  1=1.2 
DO  20  J=l,2 

FQP( I , J )=XMQQP( I ,  J  1XTHD 
20  CONTINUE 
10  CONTINUE 

CALL  MATMUL (UT ,FQP,L»M,M,P) 

CALL  MATMUL (P.UD.L.M.L.FP) 

CALL  TRANS(WTH,WTHT,N,N) 

CALL  MATMUL (HI 1 . WTHT , L  .  N, N, PI ) 

CALL  MATMUL ( PI ,  G,  L , N, L , SP  ) 

CALL  MATMUL ( UT . H21 , L . M , N , P2 ) 

CALL  MATMUL ( P2. WTHT, L . N, N, P3) 

CALL  MATMUL (P3,G,L»N.L,TP) 

CALL  TRANS ( DL1 , DL1T , N , N ) 

CALL  TRANS (WRDD.WRDDT , N, N) 

CALL  MATMUL ( WTH , DL 1 , N , N , N , P4 ) 

CALL  MATMUL ( PA, XIL,N,N,N,P5) 

CALL  MATMUL ( P5 ,DL1T,N,N,N,P6) 

CALL  MATMUL (P6 , WRDDT , N, N , N, FPF) 

CALL  TRANS ( DL ID, DL IDT , N, N) 

CALL  TRANS (WD, WDT ,  N,  N) 

CALL  MATMUL (WTH, DL1,N,N,N,P7) 

CALL  MATMUL (P7,XIL»N,N,N,P8) 

CALL  MATMUL (P8,DL1DT,N,N,N,P9) 

CALL  MATMUL ( P9 , WDT , N , N , N , FPS ) 

CALL  TRANSC ARRDD, ARRDDT ,  N,  N) 

CALL  MATMUL (ARTH,XIR,N,N,N, PI 01 
CALL  MATMUL ( P 1 0 , ARRDDT , N , N , N , FPT ) 

DO  30  1=1,3 
DO  40  J=l,3 
FPS( I ,J)=FPSCI»J)*2. 

40  CONTINUE 
30  CONTINUE 

CALL  MATADDC FPF, FPS, N, M, FPFH ) 

CALL  MATADDC  FPFH, FPT, N, N, FHP ) 

CALL  TRACE( FHP , N , TFP 1 

CALL  MATMUL (H41,DL1T,L,N,N,P11) 

CALL  MATMUL  CPU,  WTHT,  L,N,N,PI2) 

CALL  MATMUL (P12»G,L,N,L,FTHP) 

FQ=(-2.*A*MU*FP1  +  SP  +  TP  -  TFP  +  FTHP  +  TORQUE 

RETURN 

END 


SUBROUTINE  SMKN(XKN , XK1 1 , XMQQP , A , MU , THD ) 

REAL*8  XKNC2, 2) , KNPC 2, 2) , XMQQP ( 2, 2) , XK11 ( 2, 2) , A , THD, MU 
DO  10  1=1,2 
DO  20  J  =  1 , 2 

KNPC  I , J ) = XMQQP ( I , J 1 X( ”A ) *MU*( THDXX2 ) 

XKNC I , J 1 =KNP( I , J ) +XK11 ( I .  J ) 

20  CONTINUE 
10  CONTINUE 
RETURN 
END 


SUBROUTINE  SMMNQC XMNQ, DU , WTH, XIL , DL11 , DL12, W, TH , DEFM, SLOP , A , MU ) 

REAL *8  XMNQC 2, 1),DL12T(3,3),DL11T(3,3),WT(3,3),P1(3,3),P2(3,31 

REAL  *8  P3(3,3),P4(3,3),P5C3,3),P6(3,3),DL1C3,3),WTH(3,3),XIL(3,3) 

REAL  *8  W( 3 , 3) , DL11 C  3, 3 ) , DL12C  3, 3) , TH, DEFM, SLOP, A,MU 

M=2 

L  =  1 

N=3 

CALL  TRANSC  DL 11 , DL1 IT, N, N) 


ooo 


CALL  TRANSCDL12,DL12T,N,N) 

CALL  TRANS ( W, WT ,  M,  N) 

CALL  MATMUL ( WTH , DL1»N»N,N,P1) 

CALL  MATMUL CPI, XI L.N,N,N,P2) 

CALL  MATMUL  CP2, DL11T,N,N,N,P3) 

CALL  MATMULCP3,WT,N,N,N,P4) 

CALL  TRACEC  P4,N,TFP1) 

XMNQ( 1 , 1 ) =TFP1  +  C C . 35000000000000)*A*MU) 
CALL  MATMULCP2,DL12T,N,N,N,P5) 

CALL  MATMUL CP5,WT , N,N,N,P6) 

CALL  TRACEC P6 , N, TFP2) 

XMNQC  2,  1 )  =TFP2  +  C- .  05Q00000000000XAXMU) 

RETURN 

END 


20 

10 


31 


C 


SUBROUTINE  SMFNC FN , H21 , W, G, WRDD, DL1 , XI L , DL11 , DL12, WD, DL1D, H41 , TH, 
#THD, DEFM, DEFMD, SLOP.SLOPD) 

REALMS  FNC2,1),P1C2,3)»P2C3,3),P3C3,3),P4C3,3),P5C3»3),P6C3,3) 
REALMS  P7C3.3),P8<3,3),P9<3,3),P10<3,3),P11C3,3),P12C3,3),P13C3,3) 
REAL#8  P14(1»3),P15(1,3)»P16(1»3)»P17C1,3),TPC2,1)»FPC2,1)»S?(2,1) 
REALX8  FN1C3,3),FN2C3,3),GC3,1),H21(2,3),WRDDC3,3),DL1DC3,3) 

REAL *8  WDC3,3),H41C1,3),XILC3,3),W(3,3) . DL1 1 C 3, 3) , DL12C 3, 3) 

REALX8  DL1C3,3),DL11TC3,3),DL12TC3,3),WT(3,3) 

REALX8  TH, THD, DEFM, DEFMD, SLOP , SLOPD 
M=2 
L  =  1 
N=3 


CALL  TRANSCW,WT,N,N) 

CALL  MATMUL ( H21 ,WT,M,N,N,P1) 

CALL  MATMULCP1,G,M,N,L,FP) 

CALL  TRANS ( DL 11 ,DL11T,N,N) 

CALL  TRANS ( DL 12, DL12T ,N,N) 

CALL  MATMUL ( WRDD, DL1 , N , N , N , P2) 

CALL  MATMUL (P2,XIL,N,N,N,P3) 

CALL  MATMUL (P3,DL11T,N,N,N,P4) 

CALL  MATMUL  C  P4 , WT , N , N  >  N , P5 ) 

CALL  MATMUL C  WD, DL1D, N, N, N, P6 ) 

CALL  MATMUL (P6,XIL,N,N,N,P7) 

CALL  MATMUL (P7,DL11T,N,N,N,P8) 

CALL  MATMUL  C  P8 , WT ,N,N,N,P9) 

CALL  MATMUL ( P3, DL 12T , N , N , N , PI 0 ) 

CALL  MATMUL (P10,WT,N,N,N,P11) 

CALL  MATMUL ( P7 , DL12T, N, N, N , P12) 

CALL  MATMULCP12,WT,N,N,N,P13) 

DO  10  1=1,3 
DO  20  J=1 , 3 
P9(I,J)=  P9(I,J)X2. 

P13CI. J)=P13CI, J)X2. 

CONTINUE 

CONTINUE 

CALL  MATADD(P5,P9,N,N,FN1) 

CALL  MATADD(P11,P13,N,N,FN2) 

CALL  TRACEC  FN1 , N, TFN1 ) 

CALL  TRACEC  FN2, N, TFN2) 

SPC 1 , 1 ) =  TFN1 
SPC2, 1 )=TFN2 

CALL  MATMUL CH41,DL11T,L,N,N,P14) 
CALL  MATMUL  C  P14 , WT , L , N, N ,  P15 ) 

CALL  MATMUL  CP15,G,L,M,L, FN3) 

CALL  MATMUL C H41 , DL12T , L , N, N,  P16 ) 
CALL  MATMUL  CP16,WT,L,N,N,P17) 

CALL  MATMUL  CP17,G,L,N,L, FN4 ) 

TPC 1, 1 )=FN3 
TPC2 , 1 )  =FN4 
DO  31  1=1,2 

FN( 1,1) =FPC 1,1)  -  SPCI,1)  +  TPC I , 1 ) 

CONTINUE 

RETURN 

END 
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SUBROUTINE  SMMNNC  XMNN , XMQQP , ML , A . MU , XXI , YYI , MX ) 
REAL *8  XMNN(2,2),XMQQP(2,2) ,ML,MU, A, XXI, YYI, MX 
DO  10  1=1,2 
DO  20  J= 1 , 2 

XMNN(I,J)=0 . 00000000000000 
20  CONTINUE 
10  CONTINUE 

XMNNC 1,1) =ML 
XMNN( 1 , 2 ) =MX 
XMNHC2, 1 )=MX 
XMNNL2, 2) =XXI+YYI 
DO  30  1=1,2 
DO  40  J=l, 2 

XMNNC I , J ) =XMNN( I ,  J )  +  XMQQPd , J )*A#MU 
40  CONTINUE 
30  CONTINUE 
RETURN 
END 

MATRIX  MULTIPLICATION  SUBROUTINE 

SUBROUTINE  MATMUL C A, B , M, L , N, C) 

REALX8  A(M,L),B(L,N),C(M,N) 

DO  10  1=1, M 
DO  20  J  =  1 ,  N 
C(I,J)=0.0 
DO  30  INDEX=1 , L 

C( I , J ) =C( I , J )  +  ACI, INDEX )*B(IND EX, J) 

30  CONTINUE 
20  CONTINUE 
10  CONTINUE 
RETURN 
END 

MATRIX  TRANSPOSE  SUBROUTINE 

SUBROUTINE  TRANSC A, B. M, L ) 

REAL*8  A(M,L),B(L,M) 

DO  10  1=1, M 
DO  20  J=1 , L 
B(  J , I ) = A( I , J  ) 

CONTINUE 
10  CONTINUE 
RETURN 
END 

MATRIX  TRACE  SUBROUTINE 

SUBROUTINE  TRACE<A,M,TRAC) 

REALX8  A(M,M) 

TRAC=0 . 0 
DO  10  1=1, M 
TRAC=TRAC  +  A(I, I) 

10  CONTINUE 
RETURN 
END 

MATRIX  ADDITION  SUBROUTINE 

SUBROUTINE  MATADDt A , B , M, L , C) 

REAL*8  A(M,U,B(M,L),C(M,L1 
DO  10  1=1, M 
DO  20  J  =  1 ,  L 

C( I , J ) =A ( I , J  )  +  B( I , J  ) 

20  CONTINUE 
10  CONTINUE 
RETURN 
END 
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SUBROUTINE  BIGFORC BIGM. BIGF. MQQ . XMQN, FQ , XMNQ , XMNN , XKN, FN, U ) 

REAL *8  BIGMC3,3),BIGFC3,1),XMQNC1,2),XMNQC2,1),XMNNC2,2),XKNC2,2) 

REALX8  FNC2,1),UC2»1).MQ<},PC2.1),FQ 

M=2 


L  =  1 

BIGMC 1 , 1 )=MQQ 
BIGMC 1 , 2) =XMQNC 1,1) 

BIGMC 1 , 3 ) =XMQNC 1,2) 

BIGMC  2 , 1 ) =XMNQC 1,1) 
8IGMC2,2)=XMNNC1,1) 

BIGMC 2, 3) =XMNNC 1,2) 

BIGMC  3. 1 )=XMNQC2, 1 ) 

BIGMC  3, 2)=XMNNC 2,1) 

BIGMC  3, 3 ) =XMNNC  2 , 2) 

BIGFC 1 , 1 )=FQ 

CALL  MATMUL  CXKN,  U,M, M, L , P) 

BIGFC2,1)=FNC1,1)-PC1,1) 

BIGFC3,1)=FNC2,1)-PC2,1) 

RETURN 

END 


10 


SUBROUTINE  XLEQC BIGM, BIGF , SOL ) 

REALX8  BIGMC3,3),BIGFC3,1),S0LC3)» WKAREAC 18 ) 
M=1 


N=3 

CALL  LEQT2F  CBIGM,M,N,N,BIGF,M,WKAREA, IER) 

DO  10  1=1,3 

SOL  C I ) =BIGFC 1,1) 

CONTINUE 

RETURN 

END 


SUBROUTINE  GLOBCGPOS,W, DEFM) 
REALX8  GPOSC3),WC3,3), DEFM, RL C 3) 
RL(1)=1.0D0 
RL  C  2 )  =  0 . 0  DO 
RLC3)=DEFM 
N  =  3 
L  =  1 

CALL  MATMULCW,RL,N,N,L,GPOS) 
RETURN 
END 

ENDJOB 

/X 
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APPENDIX  C 


DERIVATION  OF  THE  SHAPE  FUNCTION  MATRIX 
AND  THE  NODAL  DISPLACEMENT  VECTOR 


A  cubic  shape  function  is  assumed  to  represent  the 
transverse  displacement  of  the  single  link  flexible  arm 
as  follows, 

2  3 

v  =  aQ  +  a^  x  +  a2  x  +  a3  x  -  displacement  (C.l) 

2 

$  =  ax  +  2.  a2  x  +  3.  a3  x  -  slope  (C.2) 


The  boundary  conditions  of  zero  displacement  and  slope 
at  the  base  where  x  is  equal  to  minus  the  link  length,  L, 
is  invoked.  This  is  in  accordance  with  the  positive  sign 
convention  of  the  local  coordinate  system. 

Substituting  the  boundary  conditions  into  the  shape 
functions  gives. 


2  3 

v ( -L)  =  aQ  -  a1  L  +  a2  L  -  a3  L  =  0 
$ (-L)  =  a^  -  2.  a2  L  +  3.  a3  L2  =  0 
v  (0 )  =  aQ 
<f>  (0)  =  a1 


(C .  3) 
(C .  4 ) 
(C .  5) 
(C.  6) 


Substituting  (C.5)  and  (C.6)  into  (C.3)  and  (C.4)  and 
solving  the  two  equations  for  a2  and  a3  gives: 

a2  =  (2.  MO)  L  -  3.  v(0))/'L2  (C.7) 

a3  =  (MO)  L  -  2.  v ( 0 )  )/L3  (C.8) 
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Substituting  (C.5),  (C.6),  (C.7),  (C.8)  into  (C.l)  and 


collecting  terms  gives  an  expression  for  the  transverse 
displacement  along  the  arm  length  as  a  function  of  the  arm 
tip  nodal  displacements,  v(0)  and  <M0)  , 


v ( x)  =  1.  -  (3.  x2)/L2  -  (2.  x3)/L3)  v ( 0 )  +  (x  + 


(2.  x2)  ,/L  +  (x3)/L2)  <}>  (0) 


(C .  9 ) 


Substituting  v(x)  into  the  3x1  deformation  vector 
results  in  a  3x2  shape  function  matrix  and  a  2x1  nodal 
displacement  vector  as  follows. 


r 

t 


D  =  (0,0, v(x)  )T  | 


0 

0 


:  (v(0)  ,4»(0) ) 


2x2  x3 

(x  +  *rr  +  ^ 


(C. 10) 


Since  the  expression  v' '  is  necessary  for  determining 
the  potential  energy  due  to  deformation,  the  shape  function 
matrix  is  differentiated  twice  and  results  in  the  following 
modified  shape  function  matrix. 


(0,0, v") 


i 

0 


0 


0 


(v(o) , ?  (0) ) 


(-  +  —) 
'  L  _  2 ' 

Li 


(C.ll) 
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The  theoretical  strain  is  computed  from  the  expression 
for  v' '  obtained  from  the  modified  shape  function  matrix. 
Assuming  simple  beam  theory,  v* '  is  approximated  to  equal 
the  curvature,  and  since  curvature  is  related  to  strain,  the 
following  expression  is  obtained  for  the  strain: 

e  =  c  ,  v'  '  (C . 12) 

m  1 

£  is  the  strain  at  the  maximum  distance  from  the 
m 

neutral  axis 

c^  is  the  maximum  distance  from  the  neutral  axis 


APPENDIX  D 


LINEARIZATION,  STATE  SPACE  REPRESENTATION, 

AND  OPTIMAL  CONTROLLER  DESIGN 
OF  SINGLE -LINK  DYNAMIC  EQUATIONS  ( ERLS) 

The  equations  of  motion  for  the  single-link  flexible  arm 
were  defined  as  follows: 

MQQ  9  +  MQN  U  =  F  (D.l) 

MNQ  9  +  MNN  U  +  KN  U  =  FN  (D.2) 

The  linearized  operating  point  is  defined  as  follows: 

U  =  0.0  0  =  0.0  (D . 3 ) 

U  =  0.0  0  =  0.0 

U  =  0.0  0  =0.0 

Taking  the  differential  of  each  coefficient  and  term  in 
the  equations  of  motion  and  evaluating  them  at  the  operating 
point  gives  the  fol-.owing  two  linearized  equations  of  motion. 

Fx  9  +  F2  U  =  T  (D .  4) 

F3  0  +  F4  U  +  F5  U  =  0.0  (D . 5) 

where  the  coefficients  are  the  linearized  complements  of 
the  non-linear  coefficients  as  follows: 

F1  is  from  MQQ,  F2  is  from  MQN,  and  T  is  the  applied 
torque . 

F^  is  from  MNQ,  F^  is  from  MNN,  and  Fj.  is  from  KN. 

The  state  space  variables  are  defined  as  follows: 


X1 =  e 

X2  =  9 

•  > 

II 

ro 

X 

*4  = 

( D  .  6 ) 

=  V 

x6  =  0 
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Utilizing  equations  (D.3)  through  (D.5)  and  the  state 
variable  description,  the  following  6x6  matrix  equation 
is  formed: 


F1 

F2 

0 

1 

0 

—  a 

*1 

r 

0 

0 

0 

0 

0 

*1 

0 

xl7 

fi1 

1x2 

; 

1 

X2 

0 

0 

0 

0 

-Fr 

0 

o 

F, 

0 

n 

• 

! 

1 

3 

4 

X, 

= 

0 

0 

0 

0 

2x2 

x? 

+ 

0 

2x1 

2x2 

i 

0 

0  0 

0 

0 

x4 
•  ! 

0 

1 

0 

0 

0 

0 

x4 

0 

0 

0 

0  0 

1 

0 

x5 

• 

0 

0 

1 

0 

0 

0 

X5 

0 

0 

0 

0  0 

0 

1 

X6 

0 

0 

0 

1 

0 

0 

X6 

0 

■ 

* 

w 

L 

t* 

_ 

a 

M 


(D .  7) 

Equation  (D.7)  is  then  put  into  standard  state  space 
format, 

X  =  A  X  +  B  u  (D .  8 ) 

where  A=M~^  F  and  . 

The  output  equation  having  C  as  the  identity  matrix  is, 

Y  =  C  X 

Using  the  mainframe  computer  CONTROLS  program  OPTSYS,  an 
optimal  controller  for  the  linearized  equations  of  motion  is 
designed  and  the  optimal  feedback  gain  control  matrix,  Gc, 
is  determined.  The  closed  loop  feedback  control  system  for 
the  linearized  plant  is  as  follows: 


L.  c  1 


! 

i 


n 
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